/**
 * @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;
    calcTorque = (int8_t) joy_pos;                  // joystick position
    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;
    calcPosition = (int8_t) joy_pos;
    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() {
    
}