SummerSchool2-Controller/306-controller_interface.X/app/kartculator.c

/**
 * @author R<EFBFBD>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 *= -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() {
    
}