186 lines
3.8 KiB
C
186 lines
3.8 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() {
|
|
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;
|
|
|
|
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;
|
|
} else {
|
|
calcTorque = (int8_t) joy_pos; // joystick position
|
|
}
|
|
lastPos = calcTorque;
|
|
|
|
calcTorque *= KART_CST.CONTROL_POWER_FACTOR; // convert by power factor
|
|
calcTorque /= 1000; // torque define by joystick
|
|
eKart.torque = (int16_t) calcTorque;
|
|
}
|
|
|
|
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 *= -1; // change left and right
|
|
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 *= 1000;
|
|
calcSpeed /= KART_CST.CONTROL_SPEED_FACTOR;
|
|
eKart.speed = (uint8_t) calcSpeed;
|
|
}
|
|
|
|
int16_t getTorque() {
|
|
|
|
}
|
|
|
|
uint32_t getPosition() {
|
|
|
|
}
|
|
|
|
uint8_t getSpeed() {
|
|
|
|
}
|
|
|
|
|