From 8d389cdea229532a28dcf4291a83bec959331b72 Mon Sep 17 00:00:00 2001 From: Klagarge Date: Thu, 7 Sep 2023 09:10:58 +0200 Subject: [PATCH] ramp->curve --- 306-controller_interface.X/app/car.h | 4 +- 306-controller_interface.X/app/kartculator.c | 337 ++++++++++++------- 2 files changed, 215 insertions(+), 126 deletions(-) diff --git a/306-controller_interface.X/app/car.h b/306-controller_interface.X/app/car.h index 90edcff..71f4347 100644 --- 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]; diff --git a/306-controller_interface.X/app/kartculator.c b/306-controller_interface.X/app/kartculator.c index 1895a24..f57f435 100644 --- 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;