79 lines
1.9 KiB
C
79 lines
1.9 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 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 *= (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() {
|
|
|
|
}
|
|
|
|
|