275 lines
6.6 KiB
C
275 lines
6.6 KiB
C
/**
|
|
* @author Rémi Heredero
|
|
* @version. 0.0.0
|
|
* @date August 2023
|
|
* @file kartculator.c
|
|
*/
|
|
|
|
#include "kartculator.h"
|
|
|
|
void deadJoystick(void* p){
|
|
eKart.torque = 0;
|
|
eKart.position = eKart.center;
|
|
}
|
|
|
|
void initRamp() {
|
|
rampTorque[0] = 0;
|
|
rampTorque[1] = 2;
|
|
rampTorque[2] = 5;
|
|
rampTorque[3] = 7;
|
|
rampTorque[4] = 9;
|
|
rampTorque[5] = 11;
|
|
rampTorque[6] = 13;
|
|
rampTorque[7] = 14;
|
|
rampTorque[8] = 16;
|
|
rampTorque[9] = 18;
|
|
rampTorque[10] = 19;
|
|
rampTorque[11] = 21;
|
|
rampTorque[12] = 22;
|
|
rampTorque[13] = 23;
|
|
rampTorque[14] = 24;
|
|
rampTorque[15] = 26;
|
|
rampTorque[16] = 27;
|
|
rampTorque[17] = 28;
|
|
rampTorque[18] = 29;
|
|
rampTorque[19] = 29;
|
|
rampTorque[20] = 30;
|
|
rampTorque[21] = 31;
|
|
rampTorque[22] = 32;
|
|
rampTorque[23] = 32;
|
|
rampTorque[24] = 33;
|
|
rampTorque[25] = 33;
|
|
rampTorque[26] = 34;
|
|
rampTorque[27] = 34;
|
|
rampTorque[28] = 35;
|
|
rampTorque[29] = 35;
|
|
rampTorque[30] = 35;
|
|
rampTorque[31] = 36;
|
|
rampTorque[32] = 36;
|
|
rampTorque[33] = 36;
|
|
rampTorque[34] = 36;
|
|
rampTorque[35] = 37;
|
|
rampTorque[36] = 37;
|
|
rampTorque[37] = 37;
|
|
rampTorque[38] = 37;
|
|
rampTorque[39] = 37;
|
|
rampTorque[40] = 37;
|
|
rampTorque[41] = 37;
|
|
rampTorque[42] = 37;
|
|
rampTorque[43] = 37;
|
|
rampTorque[44] = 37;
|
|
rampTorque[45] = 37;
|
|
rampTorque[46] = 37;
|
|
rampTorque[47] = 37;
|
|
rampTorque[48] = 37;
|
|
rampTorque[49] = 37;
|
|
rampTorque[50] = 38;
|
|
rampTorque[51] = 38;
|
|
rampTorque[52] = 38;
|
|
rampTorque[53] = 38;
|
|
rampTorque[54] = 38;
|
|
rampTorque[55] = 38;
|
|
rampTorque[56] = 38;
|
|
rampTorque[57] = 38;
|
|
rampTorque[58] = 38;
|
|
rampTorque[59] = 39;
|
|
rampTorque[60] = 39;
|
|
rampTorque[61] = 39;
|
|
rampTorque[62] = 39;
|
|
rampTorque[63] = 40;
|
|
rampTorque[64] = 40;
|
|
rampTorque[65] = 41;
|
|
rampTorque[66] = 41;
|
|
rampTorque[67] = 42;
|
|
rampTorque[68] = 42;
|
|
rampTorque[69] = 43;
|
|
rampTorque[70] = 44;
|
|
rampTorque[71] = 44;
|
|
rampTorque[72] = 45;
|
|
rampTorque[73] = 46;
|
|
rampTorque[74] = 47;
|
|
rampTorque[75] = 48;
|
|
rampTorque[76] = 49;
|
|
rampTorque[77] = 50;
|
|
rampTorque[78] = 51;
|
|
rampTorque[79] = 52;
|
|
rampTorque[80] = 54;
|
|
rampTorque[81] = 55;
|
|
rampTorque[82] = 57;
|
|
rampTorque[83] = 58;
|
|
rampTorque[84] = 60;
|
|
rampTorque[85] = 62;
|
|
rampTorque[86] = 64;
|
|
rampTorque[87] = 65;
|
|
rampTorque[88] = 67;
|
|
rampTorque[89] = 70;
|
|
rampTorque[90] = 72;
|
|
rampTorque[91] = 74;
|
|
rampTorque[92] = 77;
|
|
rampTorque[93] = 79;
|
|
rampTorque[94] = 82;
|
|
rampTorque[95] = 85;
|
|
rampTorque[96] = 87;
|
|
rampTorque[97] = 90;
|
|
rampTorque[98] = 93;
|
|
rampTorque[99] = 97;
|
|
rampTorque[100] = 100;
|
|
|
|
rampPosition[0] = 0;
|
|
rampPosition[1] = 0;
|
|
rampPosition[2] = 1;
|
|
rampPosition[3] = 1;
|
|
rampPosition[4] = 1;
|
|
rampPosition[5] = 1;
|
|
rampPosition[6] = 2;
|
|
rampPosition[7] = 2;
|
|
rampPosition[8] = 2;
|
|
rampPosition[9] = 3;
|
|
rampPosition[10] = 3;
|
|
rampPosition[11] = 4;
|
|
rampPosition[12] = 4;
|
|
rampPosition[13] = 4;
|
|
rampPosition[14] = 5;
|
|
rampPosition[15] = 5;
|
|
rampPosition[16] = 6;
|
|
rampPosition[17] = 6;
|
|
rampPosition[18] = 7;
|
|
rampPosition[19] = 7;
|
|
rampPosition[20] = 8;
|
|
rampPosition[21] = 8;
|
|
rampPosition[22] = 9;
|
|
rampPosition[23] = 9;
|
|
rampPosition[24] = 10;
|
|
rampPosition[25] = 11;
|
|
rampPosition[26] = 11;
|
|
rampPosition[27] = 12;
|
|
rampPosition[28] = 13;
|
|
rampPosition[29] = 13;
|
|
rampPosition[30] = 14;
|
|
rampPosition[31] = 15;
|
|
rampPosition[32] = 15;
|
|
rampPosition[33] = 16;
|
|
rampPosition[34] = 17;
|
|
rampPosition[35] = 18;
|
|
rampPosition[36] = 18;
|
|
rampPosition[37] = 19;
|
|
rampPosition[38] = 20;
|
|
rampPosition[39] = 21;
|
|
rampPosition[40] = 22;
|
|
rampPosition[41] = 23;
|
|
rampPosition[42] = 23;
|
|
rampPosition[43] = 24;
|
|
rampPosition[44] = 25;
|
|
rampPosition[45] = 26;
|
|
rampPosition[46] = 27;
|
|
rampPosition[47] = 28;
|
|
rampPosition[48] = 29;
|
|
rampPosition[49] = 30;
|
|
rampPosition[50] = 31;
|
|
rampPosition[51] = 32;
|
|
rampPosition[52] = 33;
|
|
rampPosition[53] = 34;
|
|
rampPosition[54] = 35;
|
|
rampPosition[55] = 36;
|
|
rampPosition[56] = 37;
|
|
rampPosition[57] = 38;
|
|
rampPosition[58] = 39;
|
|
rampPosition[59] = 41;
|
|
rampPosition[60] = 42;
|
|
rampPosition[61] = 43;
|
|
rampPosition[62] = 44;
|
|
rampPosition[63] = 45;
|
|
rampPosition[64] = 46;
|
|
rampPosition[65] = 48;
|
|
rampPosition[66] = 49;
|
|
rampPosition[67] = 50;
|
|
rampPosition[68] = 51;
|
|
rampPosition[69] = 53;
|
|
rampPosition[70] = 54;
|
|
rampPosition[71] = 55;
|
|
rampPosition[72] = 57;
|
|
rampPosition[73] = 58;
|
|
rampPosition[74] = 59;
|
|
rampPosition[75] = 61;
|
|
rampPosition[76] = 62;
|
|
rampPosition[77] = 63;
|
|
rampPosition[78] = 65;
|
|
rampPosition[79] = 66;
|
|
rampPosition[80] = 68;
|
|
rampPosition[81] = 69;
|
|
rampPosition[82] = 71;
|
|
rampPosition[83] = 72;
|
|
rampPosition[84] = 74;
|
|
rampPosition[85] = 75;
|
|
rampPosition[86] = 77;
|
|
rampPosition[87] = 78;
|
|
rampPosition[88] = 80;
|
|
rampPosition[89] = 82;
|
|
rampPosition[90] = 83;
|
|
rampPosition[91] = 85;
|
|
rampPosition[92] = 86;
|
|
rampPosition[93] = 88;
|
|
rampPosition[94] = 90;
|
|
rampPosition[95] = 91;
|
|
rampPosition[96] = 93;
|
|
rampPosition[97] = 95;
|
|
rampPosition[98] = 97;
|
|
rampPosition[99] = 98;
|
|
rampPosition[100] = 100;
|
|
|
|
}
|
|
|
|
void calcTorque(uint8_t joy_pos) {
|
|
int32_t calcTorque;
|
|
calcTorque = (int8_t) joy_pos; // joystick position
|
|
if(joy_pos >= 0) {
|
|
calcTorque = rampTorque[joy_pos];
|
|
} else {
|
|
calcTorque = -rampTorque[-joy_pos];
|
|
}
|
|
calcTorque *= KART_CST.CONTROL_POWER_FACTOR; // convert by power factor
|
|
calcTorque /= 1000; // torque define by joystick
|
|
eKart.torque = (int16_t) calcTorque;
|
|
}
|
|
|
|
void calcPosition(uint8_t joy_pos){
|
|
int32_t calcPosition;
|
|
calcPosition = (int8_t) joy_pos;
|
|
calcPosition *= -1; // change left and right
|
|
if(joy_pos >= 0) {
|
|
calcPosition = rampPosition[joy_pos];
|
|
} else {
|
|
calcPosition = -rampPosition[-joy_pos];
|
|
}
|
|
calcPosition *= (int32_t) KART_CST.CONTROL_STEERING_FACTOR;
|
|
calcPosition /= 1000;
|
|
calcPosition += eKart.center;
|
|
eKart.position = (uint32_t) calcPosition;
|
|
}
|
|
|
|
void calcSpeed(int32_t rpm) {
|
|
int32_t calcSpeed;
|
|
if(rpm>=0){
|
|
calcSpeed = rpm;
|
|
} else {
|
|
calcSpeed = -rpm;
|
|
}
|
|
calcSpeed *= 1000;
|
|
calcSpeed /= KART_CST.CONTROL_SPEED_FACTOR;
|
|
eKart.speed = (uint8_t) calcSpeed;
|
|
}
|
|
|
|
int16_t getTorque() {
|
|
|
|
}
|
|
|
|
uint32_t getPosition() {
|
|
|
|
}
|
|
|
|
uint8_t getSpeed() {
|
|
|
|
}
|
|
|
|
|