From 2c07512091125ef7e078e36a2322f037cacc03d2 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 23 Feb 2020 00:22:21 -0800 Subject: [PATCH 1/3] Add can bus, 2 wheel steering & stabilization, brake, dead & fault dealy --- appconf/appconf_default.h | 30 +++++++++++ applications/app_balance.c | 104 +++++++++++++++++++++++++++++-------- confgenerator.c | 30 +++++++++++ datatypes.h | 10 ++++ 4 files changed, 153 insertions(+), 21 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 190087947..902954d83 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -325,6 +325,36 @@ #ifndef APPCONF_BALANCE_CURRENT_BOOST #define APPCONF_BALANCE_CURRENT_BOOST 0.0 #endif +#ifndef APPCONF_BALANCE_MULTI_ESC +#define APPCONF_BALANCE_MULTI_ESC false +#endif +#ifndef APPCONF_BALANCE_YAW_KP +#define APPCONF_BALANCE_YAW_KP 0.0 +#endif +#ifndef APPCONF_BALANCE_YAW_KI +#define APPCONF_BALANCE_YAW_KI 0.0 +#endif +#ifndef APPCONF_BALANCE_YAW_KD +#define APPCONF_BALANCE_YAW_KD 0.0 +#endif +#ifndef APPCONF_BALANCE_ROLL_STEER_KP +#define APPCONF_BALANCE_ROLL_STEER_KP 0.0 +#endif +#ifndef APPCONF_BALANCE_BRAKE_CURRENT +#define APPCONF_BALANCE_BRAKE_CURRENT 0.0 +#endif +#ifndef APPCONF_BALANCE_OVERSPEED_DELAY +#define APPCONF_BALANCE_OVERSPEED_DELAY 0 +#endif +#ifndef APPCONF_BALANCE_FAULT_DELAY +#define APPCONF_BALANCE_FAULT_DELAY 0 +#endif +#ifndef APPCONF_BALANCE_NOSE_ANGLE +#define APPCONF_BALANCE_NOSE_ANGLE 0.0 +#endif +#ifndef APPCONF_BALANCE_NOSE_ANGLE_SPEED +#define APPCONF_BALANCE_NOSE_ANGLE_SPEED 0.0 +#endif // IMU #ifndef APPCONF_IMU_TYPE diff --git a/applications/app_balance.c b/applications/app_balance.c index a1fd359d3..dd50f9203 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -29,10 +29,14 @@ #include "imu/ahrs.h" #include "utils.h" #include "datatypes.h" +#include "comm_can.h" #include +// Can +#define MAX_CAN_AGE 0.1 + // Data type typedef enum { STARTUP = 0, @@ -57,14 +61,18 @@ static thread_t *app_thread; // Values used in loop static BalanceState state; static float pitch_angle, roll_angle; +static float gyro[3]; static float proportional, integral, derivative; static float last_proportional; static float pid_value; static float setpoint, setpoint_target; +static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value; static SetpointAdjustmentType setpointAdjustmentType; static float startup_step_size, tiltback_step_size; static systime_t current_time, last_time, diff_time; static systime_t startup_start_time, startup_diff_time; +static systime_t dead_start_time; +static systime_t fault_start_time; static uint16_t switches_value; // Values read to pass in app data to GUI @@ -111,14 +119,6 @@ void app_balance_start(void) { app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL); } -void app_balance_stop(void) { - if(app_thread != NULL){ - chThdTerminate(app_thread); - chThdWait(app_thread); - } - mc_interface_set_current(0); -} - float app_balance_get_pid_output(void) { return pid_value; } @@ -168,6 +168,47 @@ float apply_deadzone(float error){ } } +void brake(void){ + // Reset the timeout + timeout_reset(); + // Set current + mc_interface_set_brake_current(balance_conf.brake_current); + if(balance_conf.multi_esc){ + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_current_brake(msg->id, balance_conf.brake_current); + } + } + } +} + +void set_current(float current, float yaw_current){ + // Reset the timeout + timeout_reset(); + // Set current + if(balance_conf.multi_esc){ + mc_interface_set_current(current + yaw_current); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_current(msg->id, current - yaw_current); + } + } + } else { + mc_interface_set_current(current); + } +} + +void app_balance_stop(void) { + if(app_thread != NULL){ + chThdTerminate(app_thread); + chThdWait(app_thread); + } + set_current(0, 0); +} + static THD_FUNCTION(balance_thread, arg) { (void)arg; chRegSetThreadName("APP_BALANCE"); @@ -195,6 +236,7 @@ static THD_FUNCTION(balance_thread, arg) { // Get the values we want pitch_angle = imu_get_pitch() * 180.0f / M_PI; roll_angle = imu_get_roll() * 180.0f / M_PI; + imu_get_gyro(gyro); if(!balance_conf.use_switches){ switches_value = 2; @@ -214,6 +256,9 @@ static THD_FUNCTION(balance_thread, arg) { switch(state){ case (STARTUP): while(!imu_startup_done()){ + // Disable output + brake(); + // Wait chThdSleepMilliseconds(50); } state = FAULT; @@ -223,7 +268,11 @@ static THD_FUNCTION(balance_thread, arg) { case (RUNNING): // Check for overspeed if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){ - state = DEAD; + if(ST2MS(current_time - dead_start_time) > balance_conf.overspeed_delay){ + state = DEAD; + } + } else { + dead_start_time = current_time; } // Check for fault @@ -233,7 +282,11 @@ static THD_FUNCTION(balance_thread, arg) { app_balance_get_switch_value() == 0 || // Switch fully open (app_balance_get_switch_value() == 1 && fabsf(mc_interface_get_duty_cycle_now()) < 0.003) // Switch partially open and stopped ){ - state = FAULT; + if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){ + state = FAULT; + } + } else { + fault_start_time = current_time; } // Over speed tilt back safety @@ -281,15 +334,25 @@ static THD_FUNCTION(balance_thread, arg) { pid_value -= balance_conf.current_boost; } - // Reset the timeout - timeout_reset(); + if(balance_conf.multi_esc){ + // Do PID maths + if(fabsf(mc_interface_get_duty_cycle_now()) < .02){ + yaw_proportional = 0 - gyro[2]; + } else if(mc_interface_get_duty_cycle_now() < 0){ + yaw_proportional = (balance_conf.roll_steer_kp * roll_angle) - gyro[2]; + } else{ + yaw_proportional = (-balance_conf.roll_steer_kp * roll_angle) - gyro[2]; + } + yaw_integral = yaw_integral + yaw_proportional; + yaw_derivative = yaw_proportional - yaw_last_proportional; - // Output to motor - if(pid_value == 0){ - mc_interface_release_motor(); - }else { - mc_interface_set_current(pid_value); + yaw_pid_value = (balance_conf.yaw_kp * yaw_proportional) + (balance_conf.yaw_ki * yaw_integral) + (balance_conf.yaw_kd * yaw_derivative); + + yaw_last_proportional = yaw_proportional; } + + // Output to motor + set_current(pid_value, yaw_pid_value); break; case (FAULT): // Check for valid startup position and switch state @@ -300,13 +363,12 @@ static THD_FUNCTION(balance_thread, arg) { state = RUNNING; break; } - // Disable output - mc_interface_set_current(0); + brake(); break; case (DEAD): // Disable output - mc_interface_set_current(0); + brake(); break; } @@ -315,5 +377,5 @@ static THD_FUNCTION(balance_thread, arg) { } // Disable output - mc_interface_set_current(0); + brake(); } diff --git a/confgenerator.c b/confgenerator.c index 1d54bb147..3d4b7a994 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -252,6 +252,16 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_speed, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.deadzone, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.current_boost, &ind); + buffer[ind++] = conf->app_balance_conf.multi_esc; + buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_kp, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_ki, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_kd, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_steer_kp, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind); + buffer_append_uint16(buffer, conf->app_balance_conf.overspeed_delay, &ind); + buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle_speed, &ind); buffer[ind++] = conf->imu_conf.type; buffer[ind++] = conf->imu_conf.mode; buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind); @@ -530,6 +540,16 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.deadzone = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.current_boost = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.multi_esc = buffer[ind++]; + conf->app_balance_conf.yaw_kp = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.yaw_ki = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.yaw_kd = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.roll_steer_kp = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.overspeed_delay = buffer_get_uint16(buffer, &ind); + conf->app_balance_conf.fault_delay = buffer_get_uint16(buffer, &ind); + conf->app_balance_conf.nose_angle = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.nose_angle_speed = buffer_get_float32_auto(buffer, &ind); conf->imu_conf.type = buffer[ind++]; conf->imu_conf.mode = buffer[ind++]; conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind); @@ -792,6 +812,16 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.startup_speed = APPCONF_BALANCE_STARTUP_SPEED; conf->app_balance_conf.deadzone = APPCONF_BALANCE_DEADZONE; conf->app_balance_conf.current_boost = APPCONF_BALANCE_CURRENT_BOOST; + conf->app_balance_conf.multi_esc = APPCONF_BALANCE_MULTI_ESC; + conf->app_balance_conf.yaw_kp = APPCONF_BALANCE_YAW_KP; + conf->app_balance_conf.yaw_ki = APPCONF_BALANCE_YAW_KI; + conf->app_balance_conf.yaw_kd = APPCONF_BALANCE_YAW_KD; + conf->app_balance_conf.roll_steer_kp = APPCONF_BALANCE_ROLL_STEER_KP; + conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT; + conf->app_balance_conf.overspeed_delay = APPCONF_BALANCE_OVERSPEED_DELAY; + conf->app_balance_conf.fault_delay = APPCONF_BALANCE_FAULT_DELAY; + conf->app_balance_conf.nose_angle = APPCONF_BALANCE_NOSE_ANGLE; + conf->app_balance_conf.nose_angle_speed = APPCONF_BALANCE_NOSE_ANGLE_SPEED; conf->imu_conf.type = APPCONF_IMU_TYPE; conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE; conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ; diff --git a/datatypes.h b/datatypes.h index eb2e42036..c0108bba6 100644 --- a/datatypes.h +++ b/datatypes.h @@ -557,6 +557,16 @@ typedef struct { float startup_speed; float deadzone; float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float nose_angle; + float nose_angle_speed; } balance_config; // CAN status modes From d4f6036de798168d95cae374d6c750596f1e489c Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 23 Feb 2020 10:23:19 -0800 Subject: [PATCH 2/3] Implement nose angle adjustment --- appconf/appconf_default.h | 7 ++----- applications/app_balance.c | 33 ++++++++++++++++++++++++--------- confgenerator.c | 9 +++------ datatypes.h | 3 +-- 4 files changed, 30 insertions(+), 22 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 902954d83..63c76b56a 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -349,11 +349,8 @@ #ifndef APPCONF_BALANCE_FAULT_DELAY #define APPCONF_BALANCE_FAULT_DELAY 0 #endif -#ifndef APPCONF_BALANCE_NOSE_ANGLE -#define APPCONF_BALANCE_NOSE_ANGLE 0.0 -#endif -#ifndef APPCONF_BALANCE_NOSE_ANGLE_SPEED -#define APPCONF_BALANCE_NOSE_ANGLE_SPEED 0.0 +#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT +#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0 #endif // IMU diff --git a/applications/app_balance.c b/applications/app_balance.c index dd50f9203..5f56c4380 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -62,6 +62,7 @@ static thread_t *app_thread; static BalanceState state; static float pitch_angle, roll_angle; static float gyro[3]; +static float duty_cycle, abs_duty_cycle; static float proportional, integral, derivative; static float last_proportional; static float pid_value; @@ -90,6 +91,11 @@ void app_balance_start(void) { state = STARTUP; pitch_angle = 0; roll_angle = 0; + gyro[0] = 0; + gyro[1] = 0; + gyro[2] = 0; + duty_cycle = 0; + abs_duty_cycle = 0; switches_value = 0; proportional = 0; integral = 0; @@ -218,7 +224,6 @@ static THD_FUNCTION(balance_thread, arg) { tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz; state = STARTUP; - setpointAdjustmentType = CENTERING; while (!chThdShouldTerminateX()) { // Update times @@ -237,6 +242,8 @@ static THD_FUNCTION(balance_thread, arg) { pitch_angle = imu_get_pitch() * 180.0f / M_PI; roll_angle = imu_get_roll() * 180.0f / M_PI; imu_get_gyro(gyro); + duty_cycle = mc_interface_get_duty_cycle_now(); + abs_duty_cycle = fabsf(duty_cycle); if(!balance_conf.use_switches){ switches_value = 2; @@ -267,7 +274,7 @@ static THD_FUNCTION(balance_thread, arg) { break; case (RUNNING): // Check for overspeed - if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){ + if(abs_duty_cycle > balance_conf.overspeed_duty){ if(ST2MS(current_time - dead_start_time) > balance_conf.overspeed_delay){ state = DEAD; } @@ -280,7 +287,7 @@ static THD_FUNCTION(balance_thread, arg) { fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over app_balance_get_switch_value() == 0 || // Switch fully open - (app_balance_get_switch_value() == 1 && fabsf(mc_interface_get_duty_cycle_now()) < 0.003) // Switch partially open and stopped + (app_balance_get_switch_value() == 1 && abs_duty_cycle < 0.003) // Switch partially open and stopped ){ if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){ state = FAULT; @@ -290,15 +297,23 @@ static THD_FUNCTION(balance_thread, arg) { } // Over speed tilt back safety - if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.tiltback_duty || - (fabsf(mc_interface_get_duty_cycle_now()) > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage) || - (fabsf(mc_interface_get_duty_cycle_now()) > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage)){ - if(mc_interface_get_duty_cycle_now() > 0){ + if(abs_duty_cycle > balance_conf.tiltback_duty || + (abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage) || + (abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage)){ + if(duty_cycle > 0){ setpoint_target = balance_conf.tiltback_angle; } else { setpoint_target = -balance_conf.tiltback_angle; } setpointAdjustmentType = TILTBACK; + }else if(abs_duty_cycle > 0.03){ + // Nose angle adjustment + if(duty_cycle > 0){ + setpoint_target = balance_conf.tiltback_constant; + } else { + setpoint_target = -balance_conf.tiltback_constant; + } + setpointAdjustmentType = TILTBACK; }else{ setpoint_target = 0; } @@ -336,9 +351,9 @@ static THD_FUNCTION(balance_thread, arg) { if(balance_conf.multi_esc){ // Do PID maths - if(fabsf(mc_interface_get_duty_cycle_now()) < .02){ + if(abs_duty_cycle < .02){ yaw_proportional = 0 - gyro[2]; - } else if(mc_interface_get_duty_cycle_now() < 0){ + } else if(duty_cycle < 0){ yaw_proportional = (balance_conf.roll_steer_kp * roll_angle) - gyro[2]; } else{ yaw_proportional = (-balance_conf.roll_steer_kp * roll_angle) - gyro[2]; diff --git a/confgenerator.c b/confgenerator.c index 3d4b7a994..ab5b5c83f 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -260,8 +260,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind); buffer_append_uint16(buffer, conf->app_balance_conf.overspeed_delay, &ind); buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay, &ind); - buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle, &ind); - buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle_speed, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_constant, &ind); buffer[ind++] = conf->imu_conf.type; buffer[ind++] = conf->imu_conf.mode; buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind); @@ -548,8 +547,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.overspeed_delay = buffer_get_uint16(buffer, &ind); conf->app_balance_conf.fault_delay = buffer_get_uint16(buffer, &ind); - conf->app_balance_conf.nose_angle = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.nose_angle_speed = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.tiltback_constant = buffer_get_float32_auto(buffer, &ind); conf->imu_conf.type = buffer[ind++]; conf->imu_conf.mode = buffer[ind++]; conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind); @@ -820,8 +818,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT; conf->app_balance_conf.overspeed_delay = APPCONF_BALANCE_OVERSPEED_DELAY; conf->app_balance_conf.fault_delay = APPCONF_BALANCE_FAULT_DELAY; - conf->app_balance_conf.nose_angle = APPCONF_BALANCE_NOSE_ANGLE; - conf->app_balance_conf.nose_angle_speed = APPCONF_BALANCE_NOSE_ANGLE_SPEED; + conf->app_balance_conf.tiltback_constant = APPCONF_BALANCE_TILTBACK_CONSTANT; conf->imu_conf.type = APPCONF_IMU_TYPE; conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE; conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ; diff --git a/datatypes.h b/datatypes.h index c0108bba6..ce295ddee 100644 --- a/datatypes.h +++ b/datatypes.h @@ -565,8 +565,7 @@ typedef struct { float brake_current; uint16_t overspeed_delay; uint16_t fault_delay; - float nose_angle; - float nose_angle_speed; + float tiltback_constant; } balance_config; // CAN status modes From 6d29b6741f3797973f3c2419b7cdfbb3247a658b Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 23 Feb 2020 13:34:51 -0800 Subject: [PATCH 3/3] Use ADC for switches --- appconf/appconf_default.h | 7 +++- applications/app.h | 4 +- applications/app_balance.c | 75 +++++++++++++++++++++++++------------- commands.c | 4 +- confgenerator.c | 9 +++-- datatypes.h | 3 +- 6 files changed, 69 insertions(+), 33 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 63c76b56a..9e8fd6ced 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -289,8 +289,11 @@ #ifndef APPCONF_BALANCE_ROLL_FAULT #define APPCONF_BALANCE_ROLL_FAULT 45 #endif -#ifndef APPCONF_BALANCE_USE_SWITCHES -#define APPCONF_BALANCE_USE_SWITCHES false +#ifndef APPCONF_BALANCE_ADC1 +#define APPCONF_BALANCE_ADC1 0.0 +#endif +#ifndef APPCONF_BALANCE_ADC2 +#define APPCONF_BALANCE_ADC2 0.0 #endif #ifndef APPCONF_BALANCE_OVERSPEED_DUTY #define APPCONF_BALANCE_OVERSPEED_DUTY 0.9 diff --git a/applications/app.h b/applications/app.h index 1e81ea411..ee6fec131 100644 --- a/applications/app.h +++ b/applications/app.h @@ -65,7 +65,9 @@ uint32_t app_balance_get_diff_time(void); float app_balance_get_motor_current(void); float app_balance_get_motor_position(void); uint16_t app_balance_get_state(void); -uint16_t app_balance_get_switch_value(void); +uint16_t app_balance_get_switch_state(void); +float app_balance_get_adc1(void); +float app_balance_get_adc2(void); // Custom apps void app_custom_start(void); diff --git a/applications/app_balance.c b/applications/app_balance.c index 5f56c4380..c7cb7580c 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -50,6 +50,12 @@ typedef enum { TILTBACK } SetpointAdjustmentType; +typedef enum { + OFF = 0, + HALF, + ON +} SwitchState; + // Balance thread static THD_FUNCTION(balance_thread, arg); static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread @@ -74,7 +80,8 @@ static systime_t current_time, last_time, diff_time; static systime_t startup_start_time, startup_diff_time; static systime_t dead_start_time; static systime_t fault_start_time; -static uint16_t switches_value; +static float adc1, adc2; +static SwitchState switch_state; // Values read to pass in app data to GUI static float motor_current; @@ -96,7 +103,9 @@ void app_balance_start(void) { gyro[2] = 0; duty_cycle = 0; abs_duty_cycle = 0; - switches_value = 0; + adc1 = 0; + adc2 = 0; + switch_state = OFF; proportional = 0; integral = 0; derivative = 0; @@ -113,14 +122,6 @@ void app_balance_start(void) { startup_start_time = 0; startup_diff_time = 0; -#ifdef HW_SPI_PORT_SCK - // Configure pins - if(balance_conf.use_switches){ - palSetPadMode(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, PAL_MODE_INPUT_PULLDOWN); - palSetPadMode(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO, PAL_MODE_INPUT_PULLDOWN); - } -#endif - // Start the balance thread app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL); } @@ -146,8 +147,14 @@ float app_balance_get_motor_position(void) { uint16_t app_balance_get_state(void) { return state; } -uint16_t app_balance_get_switch_value(void) { - return switches_value; +uint16_t app_balance_get_switch_state(void) { + return switch_state; +} +float app_balance_get_adc1(void) { + return adc1; +} +float app_balance_get_adc2(void) { + return adc2; } float get_setpoint_adjustment_step_size(void){ @@ -244,21 +251,39 @@ static THD_FUNCTION(balance_thread, arg) { imu_get_gyro(gyro); duty_cycle = mc_interface_get_duty_cycle_now(); abs_duty_cycle = fabsf(duty_cycle); + adc1 = (((float)ADC_Value[ADC_IND_EXT])/4095) * V_REG; +#ifdef ADC_IND_EXT2 + adc2 = (((float)ADC_Value[ADC_IND_EXT2])/4095) * V_REG; +#else + adc2 = 0.0; +#endif - if(!balance_conf.use_switches){ - switches_value = 2; - }else{ - switches_value = 0; -#ifdef HW_SPI_PORT_SCK - if(palReadPad(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK)){ - switches_value += 1; + // Calculate switch state from ADC values + if(balance_conf.adc1 == 0 && balance_conf.adc2 == 0){ // No Switch + switch_state = ON; + }else if(balance_conf.adc2 == 0){ // Single switch on ADC1 + if(adc1 > balance_conf.adc1){ + switch_state = ON; + } else { + switch_state = OFF; } - if(palReadPad(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO)){ - switches_value += 1; + }else if(balance_conf.adc1 == 0){ // Single switch on ADC2 + if(adc2 > balance_conf.adc2){ + switch_state = ON; + } else { + switch_state = OFF; + } + }else{ // Double switch + if(adc1 > balance_conf.adc1 && adc2 > balance_conf.adc2){ + switch_state = ON; + }else if(adc1 > balance_conf.adc1 || adc2 > balance_conf.adc2){ + switch_state = HALF; + }else{ + switch_state = OFF; } -#endif } + // State based logic switch(state){ case (STARTUP): @@ -286,8 +311,8 @@ static THD_FUNCTION(balance_thread, arg) { if( fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over - app_balance_get_switch_value() == 0 || // Switch fully open - (app_balance_get_switch_value() == 1 && abs_duty_cycle < 0.003) // Switch partially open and stopped + switch_state == OFF || // Switch fully open + (switch_state == HALF && abs_duty_cycle < 0.003) // Switch partially open and stopped ){ if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){ state = FAULT; @@ -371,7 +396,7 @@ static THD_FUNCTION(balance_thread, arg) { break; case (FAULT): // Check for valid startup position and switch state - if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && app_balance_get_switch_value() == 2){ + if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && switch_state == ON){ setpoint = pitch_angle; setpoint_target = 0; setpointAdjustmentType = CENTERING; diff --git a/commands.c b/commands.c index 650af0955..bf128eb40 100644 --- a/commands.c +++ b/commands.c @@ -547,7 +547,9 @@ void commands_process_packet(unsigned char *data, unsigned int len, buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &ind); buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_position() * 1000000.0), &ind); buffer_append_uint16(send_buffer, app_balance_get_state(), &ind); - buffer_append_uint16(send_buffer, app_balance_get_switch_value(), &ind); + buffer_append_uint16(send_buffer, app_balance_get_switch_state(), &ind); + buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc1() * 1000000.0), &ind); + buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc2() * 1000000.0), &ind); reply_func(send_buffer, ind); } break; diff --git a/confgenerator.c b/confgenerator.c index ab5b5c83f..fb5848a3b 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -240,7 +240,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); - buffer[ind++] = conf->app_balance_conf.use_switches; + buffer_append_float32_auto(buffer, conf->app_balance_conf.adc1, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.adc2, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_angle, &ind); @@ -527,7 +528,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind); conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.use_switches = buffer[ind++]; + conf->app_balance_conf.adc1 = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.adc2 = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.tiltback_angle = buffer_get_float32_auto(buffer, &ind); @@ -798,7 +800,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ; conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; - conf->app_balance_conf.use_switches = APPCONF_BALANCE_USE_SWITCHES; + conf->app_balance_conf.adc1 = APPCONF_BALANCE_ADC1; + conf->app_balance_conf.adc2 = APPCONF_BALANCE_ADC2; conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY; conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY; conf->app_balance_conf.tiltback_angle = APPCONF_BALANCE_TILTBACK_ANGLE; diff --git a/datatypes.h b/datatypes.h index ce295ddee..5ba589409 100644 --- a/datatypes.h +++ b/datatypes.h @@ -545,7 +545,8 @@ typedef struct { uint16_t hertz; float pitch_fault; float roll_fault; - bool use_switches; + float adc1; + float adc2; float overspeed_duty; float tiltback_duty; float tiltback_angle;