diff --git a/306-controller_interface.X/app/car.h b/306-controller_interface.X/app/car.h index 9a44e0d..b81fe60 100644 --- a/306-controller_interface.X/app/car.h +++ b/306-controller_interface.X/app/car.h @@ -43,6 +43,8 @@ #define MEMADD_BATTERY_CURRENT_TIME 0x1B #define MEMADD_BATTERY_ENERGY_TIME 0x1C #define MEMADD_BATTERY_ALIVE_TIME 0x1D +#define MEMADD_CONTROL_PARAM_MAX_CHANGE_STEERING 0x1E +#define MEMADD_CONTROL_PARAM_MAX_CHANGE_DRIVE 0x1F const bool ALWAYSTRUE = true; const bool ALWAYSFALSE = false; @@ -72,6 +74,9 @@ typedef struct { uint8_t BATTERY_CURRENT_TIME; uint8_t BATTERY_ENERGY_TIME; uint8_t BATTERY_ALIVE_TIME; + uint8_t CONTROL_PARAM_MAX_CHANGE_STEERING; + uint8_t CONTROL_PARAM_MAX_CHANGE_DRIVE; + } KART_CST_TYPE; KART_CST_TYPE KART_CST; diff --git a/306-controller_interface.X/app/eeprom.c b/306-controller_interface.X/app/eeprom.c index 013c1b9..0e938d1 100644 --- a/306-controller_interface.X/app/eeprom.c +++ b/306-controller_interface.X/app/eeprom.c @@ -49,6 +49,9 @@ void MEM_init(){ KART_CST.BATTERY_ENERGY_TIME = 50; KART_CST.BATTERY_ALIVE_TIME = 50; + KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING = 10; + KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE = 10; + MEM_write_1_byte(MEMADD_CONTROL_STEERING_MODE, KART_CST.CONTROL_STEERING_MODE); MEM_write_1_byte(MEMADD_CONTROL_ALIVE_TIME, KART_CST.CONTROL_ALIVE_TIME); MEM_write_4_byte(MEMADD_CONTROL_SPEED_FACTOR, KART_CST.CONTROL_SPEED_FACTOR); @@ -75,6 +78,9 @@ void MEM_init(){ MEM_write_1_byte(MEMADD_BATTERY_ENERGY_TIME, KART_CST.BATTERY_ENERGY_TIME); MEM_write_1_byte(MEMADD_BATTERY_ALIVE_TIME, KART_CST.BATTERY_ALIVE_TIME); + MEM_write_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_DRIVE, KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE); + MEM_write_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_STEERING, KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING); + MEM_write_1_byte(0x0, 0x42); } else { KART_CST.CONTROL_STEERING_MODE = MEM_read_1_byte(MEMADD_CONTROL_STEERING_MODE); @@ -103,8 +109,10 @@ void MEM_init(){ KART_CST.BATTERY_ENERGY_TIME = MEM_read_1_byte(MEMADD_BATTERY_ENERGY_TIME); KART_CST.BATTERY_ALIVE_TIME = MEM_read_1_byte(MEMADD_BATTERY_ALIVE_TIME); + KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE = MEM_read_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_DRIVE); + KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING = MEM_read_1_byte(MEMADD_CONTROL_PARAM_MAX_CHANGE_STEERING); + } - CAN_Send(0, 5, KART_CST.CONTROL_POWER_FACTOR); } void MEM_reset() { diff --git a/306-controller_interface.X/app/kartculator.c b/306-controller_interface.X/app/kartculator.c index 40f2b5c..a36ae22 100644 --- a/306-controller_interface.X/app/kartculator.c +++ b/306-controller_interface.X/app/kartculator.c @@ -11,9 +11,21 @@ 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 + 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; @@ -21,7 +33,18 @@ void calcTorque(uint8_t joy_pos) { void calcPosition(uint8_t joy_pos){ int32_t calcPosition; - calcPosition = (int8_t) joy_pos; + 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;