Klagarge/SummerSchool2-Controller
Klagarge/SummerSchool2-Controller
Archived
1
0
Fork 0
Code Issues Pull Requests Projects 1 Releases 1 Activity

ramp->curve

Browse Source
This commit is contained in:
Rémi Heredero 2023-09-07 09:10:58 +02:00
parent cc6a8fc39a
commit 8d389cdea2
2 changed files with 215 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
306-controller_interface.X/app/car.h
View File

@ -103,8 +103,8 @@ typedef struct {
} KART_VAR_TYPE;
KART_VAR_TYPE eKart;
uint8_t exp[100];
uint8_t log[100];
uint8_t rampTorque[101];
uint8_t rampPosition[101];

337
306-controller_interface.X/app/kartculator.c
View File

@ -13,125 +13,220 @@ void deadJoystick(void* p){
}
void initRamp() {
exp[0] = 0;
exp[1] = 0;
exp[2] = 0;
exp[3] = 0;
exp[4] = 0;
exp[5] = 0;
exp[6] = 0;
exp[7] = 0;
exp[8] = 0;
exp[9] = 0;
exp[10] = 1;
exp[11] = 1;
exp[12] = 1;
exp[13] = 1;
exp[14] = 1;
exp[15] = 1;
exp[16] = 1;
exp[17] = 1;
exp[18] = 1;
exp[19] = 1;
exp[20] = 1;
exp[21] = 2;
exp[22] = 2;
exp[23] = 2;
exp[24] = 2;
exp[25] = 2;
exp[26] = 2;
exp[27] = 2;
exp[28] = 2;
exp[29] = 3;
exp[30] = 3;
exp[31] = 3;
exp[32] = 3;
exp[33] = 3;
exp[34] = 4;
exp[35] = 4;
exp[36] = 4;
exp[37] = 4;
exp[38] = 5;
exp[39] = 5;
exp[40] = 5;
exp[41] = 6;
exp[42] = 6;
exp[43] = 6;
exp[44] = 7;
exp[45] = 7;
exp[46] = 7;
exp[47] = 8;
exp[48] = 8;
exp[49] = 9;
exp[50] = 9;
exp[51] = 10;
exp[52] = 10;
exp[53] = 11;
exp[54] = 11;
exp[55] = 12;
exp[56] = 12;
exp[57] = 13;
exp[58] = 14;
exp[59] = 14;
exp[60] = 15;
exp[61] = 16;
exp[62] = 16;
exp[63] = 17;
exp[64] = 18;
exp[65] = 19;
exp[66] = 20;
exp[67] = 21;
exp[68] = 22;
exp[69] = 23;
exp[70] = 24;
exp[71] = 25;
exp[72] = 27;
exp[73] = 28;
exp[74] = 29;
exp[75] = 31;
exp[76] = 32;
exp[77] = 34;
exp[78] = 36;
exp[79] = 37;
exp[80] = 39;
exp[81] = 41;
exp[82] = 43;
exp[83] = 45;
exp[84] = 47;
exp[85] = 50;
exp[86] = 52;
exp[87] = 54;
exp[88] = 57;
exp[89] = 60;
exp[90] = 63;
exp[91] = 66;
exp[92] = 69;
exp[93] = 72;
exp[94] = 76;
exp[95] = 79;
exp[96] = 83;
exp[97] = 87;
exp[98] = 91;
exp[99] = 95;
exp[100] = 100;
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;
log[0] = 0;
}
void calcTorque(uint8_t joy_pos) {
int32_t calcTorque;
static int8_t lastPos = 0;
int8_t diff = ((int8_t)joy_pos) - lastPos;
if (diff > KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE) {
calcTorque = lastPos + KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE;
} else if (diff < -KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE) {
calcTorque = lastPos - KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE;
calcTorque = (int8_t) joy_pos; // joystick position
if(joy_pos >= 0) {
calcTorque = rampTorque[joy_pos];
} else {
calcTorque = (int8_t) joy_pos; // joystick position
calcTorque = -rampTorque[-joy_pos];
}
lastPos = calcTorque;
calcTorque *= KART_CST.CONTROL_POWER_FACTOR; // convert by power factor
calcTorque /= 1000; // torque define by joystick
eKart.torque = (int16_t) calcTorque;
@ -139,19 +234,13 @@ void calcTorque(uint8_t joy_pos) {
void calcPosition(uint8_t joy_pos){
int32_t calcPosition;
static int8_t lastPos = 0;
int8_t diff = ((int8_t)joy_pos) - lastPos;
if (diff > KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING) {
calcPosition = lastPos + KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING;
} else if (diff < -KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING) {
calcPosition = lastPos - KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING;
} else {
calcPosition = (int8_t) joy_pos;
}
lastPos = 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;