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;
KART_VAR_TYPE eKart; KART_VAR_TYPE eKart;
uint8_t exp[100]; uint8_t rampTorque[101];
uint8_t log[100]; uint8_t rampPosition[101];

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

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