300 lines
7.1 KiB
C
300 lines
7.1 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] = 3;
|
||
rampTorque[2] = 5;
|
||
rampTorque[3] = 7;
|
||
rampTorque[4] = 10;
|
||
rampTorque[5] = 12;
|
||
rampTorque[6] = 14;
|
||
rampTorque[7] = 16;
|
||
rampTorque[8] = 18;
|
||
rampTorque[9] = 20;
|
||
rampTorque[10] = 22;
|
||
rampTorque[11] = 24;
|
||
rampTorque[12] = 25;
|
||
rampTorque[13] = 27;
|
||
rampTorque[14] = 28;
|
||
rampTorque[15] = 30;
|
||
rampTorque[16] = 31;
|
||
rampTorque[17] = 33;
|
||
rampTorque[18] = 34;
|
||
rampTorque[19] = 36;
|
||
rampTorque[20] = 37;
|
||
rampTorque[21] = 38;
|
||
rampTorque[22] = 39;
|
||
rampTorque[23] = 40;
|
||
rampTorque[24] = 41;
|
||
rampTorque[25] = 42;
|
||
rampTorque[26] = 43;
|
||
rampTorque[27] = 44;
|
||
rampTorque[28] = 45;
|
||
rampTorque[29] = 46;
|
||
rampTorque[30] = 47;
|
||
rampTorque[31] = 47;
|
||
rampTorque[32] = 48;
|
||
rampTorque[33] = 49;
|
||
rampTorque[34] = 50;
|
||
rampTorque[35] = 50;
|
||
rampTorque[36] = 51;
|
||
rampTorque[37] = 51;
|
||
rampTorque[38] = 52;
|
||
rampTorque[39] = 53;
|
||
rampTorque[40] = 53;
|
||
rampTorque[41] = 53;
|
||
rampTorque[42] = 54;
|
||
rampTorque[43] = 54;
|
||
rampTorque[44] = 55;
|
||
rampTorque[45] = 55;
|
||
rampTorque[46] = 56;
|
||
rampTorque[47] = 56;
|
||
rampTorque[48] = 56;
|
||
rampTorque[49] = 57;
|
||
rampTorque[50] = 57;
|
||
rampTorque[51] = 58;
|
||
rampTorque[52] = 58;
|
||
rampTorque[53] = 58;
|
||
rampTorque[54] = 59;
|
||
rampTorque[55] = 59;
|
||
rampTorque[56] = 59;
|
||
rampTorque[57] = 60;
|
||
rampTorque[58] = 60;
|
||
rampTorque[59] = 60;
|
||
rampTorque[60] = 61;
|
||
rampTorque[61] = 61;
|
||
rampTorque[62] = 62;
|
||
rampTorque[63] = 62;
|
||
rampTorque[64] = 62;
|
||
rampTorque[65] = 63;
|
||
rampTorque[66] = 63;
|
||
rampTorque[67] = 64;
|
||
rampTorque[68] = 64;
|
||
rampTorque[69] = 65;
|
||
rampTorque[70] = 65;
|
||
rampTorque[71] = 66;
|
||
rampTorque[72] = 67;
|
||
rampTorque[73] = 67;
|
||
rampTorque[74] = 68;
|
||
rampTorque[75] = 69;
|
||
rampTorque[76] = 69;
|
||
rampTorque[77] = 70;
|
||
rampTorque[78] = 71;
|
||
rampTorque[79] = 72;
|
||
rampTorque[80] = 72;
|
||
rampTorque[81] = 73;
|
||
rampTorque[82] = 74;
|
||
rampTorque[83] = 75;
|
||
rampTorque[84] = 76;
|
||
rampTorque[85] = 77;
|
||
rampTorque[86] = 78;
|
||
rampTorque[87] = 80;
|
||
rampTorque[88] = 81;
|
||
rampTorque[89] = 82;
|
||
rampTorque[90] = 83;
|
||
rampTorque[91] = 85;
|
||
rampTorque[92] = 86;
|
||
rampTorque[93] = 88;
|
||
rampTorque[94] = 89;
|
||
rampTorque[95] = 91;
|
||
rampTorque[96] = 93;
|
||
rampTorque[97] = 94;
|
||
rampTorque[98] = 96;
|
||
rampTorque[99] = 98;
|
||
rampTorque[100] = 100;
|
||
|
||
rampPosition[0] = 0;
|
||
rampPosition[1] = 0;
|
||
rampPosition[2] = 0;
|
||
rampPosition[3] = 0;
|
||
rampPosition[4] = 0;
|
||
rampPosition[5] = 1;
|
||
rampPosition[6] = 1;
|
||
rampPosition[7] = 1;
|
||
rampPosition[8] = 2;
|
||
rampPosition[9] = 2;
|
||
rampPosition[10] = 3;
|
||
rampPosition[11] = 3;
|
||
rampPosition[12] = 4;
|
||
rampPosition[13] = 5;
|
||
rampPosition[14] = 5;
|
||
rampPosition[15] = 6;
|
||
rampPosition[16] = 7;
|
||
rampPosition[17] = 8;
|
||
rampPosition[18] = 9;
|
||
rampPosition[19] = 9;
|
||
rampPosition[20] = 10;
|
||
rampPosition[21] = 11;
|
||
rampPosition[22] = 12;
|
||
rampPosition[23] = 13;
|
||
rampPosition[24] = 15;
|
||
rampPosition[25] = 16;
|
||
rampPosition[26] = 17;
|
||
rampPosition[27] = 18;
|
||
rampPosition[28] = 19;
|
||
rampPosition[29] = 20;
|
||
rampPosition[30] = 22;
|
||
rampPosition[31] = 23;
|
||
rampPosition[32] = 24;
|
||
rampPosition[33] = 25;
|
||
rampPosition[34] = 27;
|
||
rampPosition[35] = 28;
|
||
rampPosition[36] = 30;
|
||
rampPosition[37] = 31;
|
||
rampPosition[38] = 32;
|
||
rampPosition[39] = 34;
|
||
rampPosition[40] = 35;
|
||
rampPosition[41] = 37;
|
||
rampPosition[42] = 38;
|
||
rampPosition[43] = 40;
|
||
rampPosition[44] = 41;
|
||
rampPosition[45] = 43;
|
||
rampPosition[46] = 44;
|
||
rampPosition[47] = 46;
|
||
rampPosition[48] = 47;
|
||
rampPosition[49] = 49;
|
||
rampPosition[50] = 50;
|
||
rampPosition[51] = 51;
|
||
rampPosition[52] = 53;
|
||
rampPosition[53] = 54;
|
||
rampPosition[54] = 56;
|
||
rampPosition[55] = 57;
|
||
rampPosition[56] = 59;
|
||
rampPosition[57] = 60;
|
||
rampPosition[58] = 62;
|
||
rampPosition[59] = 63;
|
||
rampPosition[60] = 65;
|
||
rampPosition[61] = 66;
|
||
rampPosition[62] = 68;
|
||
rampPosition[63] = 69;
|
||
rampPosition[64] = 70;
|
||
rampPosition[65] = 72;
|
||
rampPosition[66] = 73;
|
||
rampPosition[67] = 75;
|
||
rampPosition[68] = 76;
|
||
rampPosition[69] = 77;
|
||
rampPosition[70] = 78;
|
||
rampPosition[71] = 80;
|
||
rampPosition[72] = 81;
|
||
rampPosition[73] = 82;
|
||
rampPosition[74] = 83;
|
||
rampPosition[75] = 84;
|
||
rampPosition[76] = 85;
|
||
rampPosition[77] = 87;
|
||
rampPosition[78] = 88;
|
||
rampPosition[79] = 89;
|
||
rampPosition[80] = 90;
|
||
rampPosition[81] = 91;
|
||
rampPosition[82] = 91;
|
||
rampPosition[83] = 92;
|
||
rampPosition[84] = 93;
|
||
rampPosition[85] = 94;
|
||
rampPosition[86] = 95;
|
||
rampPosition[87] = 95;
|
||
rampPosition[88] = 96;
|
||
rampPosition[89] = 97;
|
||
rampPosition[90] = 97;
|
||
rampPosition[91] = 98;
|
||
rampPosition[92] = 98;
|
||
rampPosition[93] = 99;
|
||
rampPosition[94] = 99;
|
||
rampPosition[95] = 99;
|
||
rampPosition[96] = 100;
|
||
rampPosition[97] = 100;
|
||
rampPosition[98] = 100;
|
||
rampPosition[99] = 100;
|
||
rampPosition[100] = 100;
|
||
|
||
|
||
}
|
||
|
||
void calcTorque(uint8_t joy_pos) {
|
||
int32_t calcTorque;
|
||
calcTorque = (int8_t) joy_pos; // joystick position
|
||
|
||
// if mode race, use special curve for torque
|
||
if(eKart.powerMode == 2) {
|
||
if(calcTorque >= 0) {
|
||
calcTorque = rampTorque[calcTorque];
|
||
} else {
|
||
calcTorque *= -1;
|
||
calcTorque = rampTorque[calcTorque];
|
||
calcTorque *= -1;
|
||
}
|
||
}
|
||
|
||
calcTorque *= KART_CST.CONTROL_POWER_FACTOR; // convert by power factor
|
||
calcTorque /= 1000; // torque define by joystick
|
||
|
||
if(eKart.button) {
|
||
calcTorque *= 1150;
|
||
calcTorque /= 1000;
|
||
}
|
||
|
||
if(eKart.batteryVoltage <= 18750) {
|
||
calcTorque = 0;
|
||
}
|
||
|
||
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 mode race, use special curve for steering
|
||
if (eKart.powerMode == 2) {
|
||
if(calcPosition >= 0) {
|
||
calcPosition = rampPosition[calcPosition];
|
||
} else {
|
||
calcPosition *= -1;
|
||
calcPosition = rampPosition[calcPosition];
|
||
calcPosition *= -1;
|
||
}
|
||
}
|
||
|
||
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 *= 2000;
|
||
calcSpeed /= KART_CST.CONTROL_SPEED_FACTOR;
|
||
eKart.speed = (uint8_t) calcSpeed;
|
||
}
|
||
|
||
int16_t getTorque() {
|
||
|
||
}
|
||
|
||
uint32_t getPosition() {
|
||
|
||
}
|
||
|
||
uint8_t getSpeed() {
|
||
|
||
}
|
||
|
||
|