ramp->curve

2023-09-07 09:10:58 +02:00
parent cc6a8fc39a
commit 8d389cdea2
2 changed files with 215 additions and 126 deletions
--- a/306-controller_interface.X/app/car.h
+++ b/306-controller_interface.X/app/car.h
@@ -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];



--- a/306-controller_interface.X/app/kartculator.c
+++ b/306-controller_interface.X/app/kartculator.c
@@ -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;