2019-07-27 21:25:56 -07:00
|
|
|
/*
|
|
|
|
Copyright 2019 Mitch Lustig
|
|
|
|
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "conf_general.h"
|
|
|
|
|
|
|
|
#include "ch.h" // ChibiOS
|
|
|
|
#include "hal.h" // ChibiOS HAL
|
|
|
|
#include "mc_interface.h" // Motor control functions
|
|
|
|
#include "hw.h" // Pin mapping on this hardware
|
|
|
|
#include "timeout.h" // To reset the timeout
|
|
|
|
#include "commands.h"
|
|
|
|
#include "imu/imu.h"
|
2019-08-04 11:47:23 -07:00
|
|
|
#include "imu/ahrs.h"
|
2019-08-07 00:22:51 -07:00
|
|
|
#include "utils.h"
|
2019-09-08 18:32:53 -07:00
|
|
|
#include "datatypes.h"
|
2019-08-07 00:22:51 -07:00
|
|
|
|
2019-07-27 21:25:56 -07:00
|
|
|
|
|
|
|
#include <math.h>
|
|
|
|
|
2019-08-04 11:47:23 -07:00
|
|
|
// Data type
|
|
|
|
typedef enum {
|
2019-08-27 19:57:35 -07:00
|
|
|
STARTUP = 0,
|
2019-08-04 11:47:23 -07:00
|
|
|
RUNNING,
|
2019-08-07 00:22:51 -07:00
|
|
|
FAULT,
|
|
|
|
DEAD
|
2019-08-04 11:47:23 -07:00
|
|
|
} BalanceState;
|
|
|
|
|
2019-08-04 23:13:35 -07:00
|
|
|
typedef enum {
|
2019-08-27 19:57:35 -07:00
|
|
|
CENTERING = 0,
|
2019-08-07 00:22:51 -07:00
|
|
|
TILTBACK
|
|
|
|
} SetpointAdjustmentType;
|
2019-08-04 23:13:35 -07:00
|
|
|
|
2019-08-10 22:00:00 -07:00
|
|
|
// Balance thread
|
|
|
|
static THD_FUNCTION(balance_thread, arg);
|
|
|
|
static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2019-08-27 09:10:44 -07:00
|
|
|
static volatile balance_config balance_conf;
|
2019-07-27 21:25:56 -07:00
|
|
|
static thread_t *app_thread;
|
|
|
|
|
2019-07-29 00:31:48 -07:00
|
|
|
// Values used in loop
|
2019-08-04 11:47:23 -07:00
|
|
|
static BalanceState state;
|
2019-09-08 18:32:53 -07:00
|
|
|
static float m_angle, c_angle;
|
2019-08-10 00:16:48 -07:00
|
|
|
static float proportional, integral, derivative;
|
|
|
|
static float last_proportional;
|
|
|
|
static float pid_value;
|
|
|
|
static float setpoint, setpoint_target;
|
2019-08-07 00:22:51 -07:00
|
|
|
static SetpointAdjustmentType setpointAdjustmentType;
|
2019-08-10 00:16:48 -07:00
|
|
|
static float startup_step_size, tiltback_step_size;
|
2019-08-04 23:13:35 -07:00
|
|
|
static systime_t current_time, last_time, diff_time;
|
2019-08-27 19:57:35 -07:00
|
|
|
static systime_t startup_start_time, startup_diff_time;
|
2019-07-29 00:31:48 -07:00
|
|
|
|
|
|
|
// Values read to pass in app data to GUI
|
2019-08-10 00:16:48 -07:00
|
|
|
static float motor_current;
|
|
|
|
static float motor_position;
|
2019-07-29 00:31:48 -07:00
|
|
|
|
2019-09-08 18:32:53 -07:00
|
|
|
void app_balance_configure(balance_config *conf) {
|
2019-08-27 09:10:44 -07:00
|
|
|
balance_conf = *conf;
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
void app_balance_start(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
|
|
|
|
// Reset all Values
|
2019-08-27 19:57:35 -07:00
|
|
|
state = STARTUP;
|
2019-09-08 18:32:53 -07:00
|
|
|
m_angle = 0;
|
|
|
|
c_angle = 0;
|
2019-08-07 00:22:51 -07:00
|
|
|
proportional = 0;
|
|
|
|
integral = 0;
|
|
|
|
derivative = 0;
|
|
|
|
last_proportional = 0;
|
|
|
|
pid_value = 0;
|
|
|
|
setpoint = 0;
|
|
|
|
setpoint_target = 0;
|
2019-08-27 19:57:35 -07:00
|
|
|
setpointAdjustmentType = CENTERING;
|
2019-08-08 23:39:14 -07:00
|
|
|
startup_step_size = 0;
|
|
|
|
tiltback_step_size = 0;
|
2019-08-10 00:16:48 -07:00
|
|
|
current_time = 0;
|
|
|
|
last_time = 0;
|
|
|
|
diff_time = 0;
|
2019-08-27 19:57:35 -07:00
|
|
|
startup_start_time = 0;
|
|
|
|
startup_diff_time = 0;
|
2019-07-30 00:27:23 -07:00
|
|
|
|
2019-08-10 22:00:00 -07:00
|
|
|
// Start the balance thread
|
|
|
|
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
void app_balance_stop(void) {
|
2019-08-10 21:52:21 -07:00
|
|
|
if(app_thread != NULL){
|
|
|
|
chThdTerminate(app_thread);
|
|
|
|
chThdWait(app_thread);
|
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
mc_interface_set_current(0);
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
float app_balance_get_pid_output(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
return pid_value;
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
2019-09-08 22:23:31 -07:00
|
|
|
float app_balance_get_m_angle(void) {
|
2019-09-08 18:32:53 -07:00
|
|
|
return m_angle;
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
2019-09-08 22:23:31 -07:00
|
|
|
float app_balance_get_c_angle(void) {
|
2019-09-08 18:32:53 -07:00
|
|
|
return c_angle;
|
2019-07-30 00:27:23 -07:00
|
|
|
}
|
|
|
|
uint32_t app_balance_get_diff_time(void) {
|
2019-08-10 00:16:48 -07:00
|
|
|
return ST2US(diff_time);
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
|
|
|
float app_balance_get_motor_current(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
return motor_current;
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
2019-07-30 00:27:23 -07:00
|
|
|
float app_balance_get_motor_position(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
return motor_position;
|
|
|
|
}
|
2019-08-09 20:55:58 -07:00
|
|
|
uint16_t app_balance_get_state(void) {
|
|
|
|
return state;
|
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
|
2019-08-10 00:16:48 -07:00
|
|
|
float get_setpoint_adjustment_step_size(void){
|
2019-08-07 00:22:51 -07:00
|
|
|
switch(setpointAdjustmentType){
|
2019-08-27 19:57:35 -07:00
|
|
|
case (CENTERING):
|
2019-08-07 00:22:51 -07:00
|
|
|
return startup_step_size;
|
|
|
|
case (TILTBACK):
|
|
|
|
return tiltback_step_size;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-08-10 00:16:48 -07:00
|
|
|
float apply_deadzone(float error){
|
2019-08-27 09:10:44 -07:00
|
|
|
if(balance_conf.deadzone == 0){
|
2019-08-07 00:22:51 -07:00
|
|
|
return error;
|
|
|
|
}
|
|
|
|
|
2019-08-27 09:10:44 -07:00
|
|
|
if(error < balance_conf.deadzone && error > -balance_conf.deadzone){
|
2019-08-07 00:22:51 -07:00
|
|
|
return 0;
|
2019-08-27 09:10:44 -07:00
|
|
|
} else if(error > balance_conf.deadzone){
|
|
|
|
return error - balance_conf.deadzone;
|
2019-08-07 00:22:51 -07:00
|
|
|
} else {
|
2019-08-27 09:10:44 -07:00
|
|
|
return error + balance_conf.deadzone;
|
2019-08-07 00:22:51 -07:00
|
|
|
}
|
2019-07-30 00:27:23 -07:00
|
|
|
}
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2019-08-10 22:00:00 -07:00
|
|
|
static THD_FUNCTION(balance_thread, arg) {
|
2019-07-27 21:25:56 -07:00
|
|
|
(void)arg;
|
2019-08-07 00:22:51 -07:00
|
|
|
chRegSetThreadName("APP_BALANCE");
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2019-08-08 23:39:14 -07:00
|
|
|
// Do one off config
|
2019-08-27 09:10:44 -07:00
|
|
|
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
|
|
|
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
|
2019-08-08 23:39:14 -07:00
|
|
|
|
2019-08-27 19:57:35 -07:00
|
|
|
state = STARTUP;
|
|
|
|
setpointAdjustmentType = CENTERING;
|
2019-08-08 23:39:14 -07:00
|
|
|
|
2019-07-27 21:25:56 -07:00
|
|
|
while (!chThdShouldTerminateX()) {
|
2019-08-04 11:47:23 -07:00
|
|
|
// Update times
|
|
|
|
current_time = chVTGetSystemTimeX();
|
2019-08-10 00:16:48 -07:00
|
|
|
if(last_time == 0){
|
2019-08-04 11:47:23 -07:00
|
|
|
last_time = current_time;
|
|
|
|
}
|
|
|
|
diff_time = current_time - last_time;
|
|
|
|
last_time = current_time;
|
|
|
|
|
|
|
|
// Read values for GUI
|
|
|
|
motor_current = mc_interface_get_tot_current_directional_filtered();
|
|
|
|
motor_position = mc_interface_get_pid_pos_now();
|
|
|
|
|
2019-09-08 18:32:53 -07:00
|
|
|
// Get the values we want
|
|
|
|
switch(balance_conf.m_axis){
|
|
|
|
case (PITCH):
|
|
|
|
m_angle = imu_get_pitch() * 180.0f / M_PI;;
|
|
|
|
break;
|
|
|
|
case (ROLL):
|
|
|
|
m_angle = imu_get_roll() * 180.0f / M_PI;
|
|
|
|
break;
|
|
|
|
case (YAW):
|
|
|
|
m_angle = imu_get_yaw() * 180.0f / M_PI;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
switch(balance_conf.c_axis){
|
|
|
|
case (PITCH):
|
|
|
|
c_angle = imu_get_pitch() * 180.0f / M_PI;;
|
|
|
|
break;
|
|
|
|
case (ROLL):
|
|
|
|
c_angle = imu_get_roll() * 180.0f / M_PI;
|
|
|
|
break;
|
|
|
|
case (YAW):
|
|
|
|
c_angle = imu_get_yaw() * 180.0f / M_PI;
|
|
|
|
break;
|
|
|
|
}
|
2019-08-04 11:47:23 -07:00
|
|
|
|
|
|
|
// State based logic
|
|
|
|
switch(state){
|
2019-08-27 19:57:35 -07:00
|
|
|
case (STARTUP):
|
|
|
|
if(startup_start_time == 0){
|
|
|
|
startup_start_time = current_time;
|
2019-08-27 09:10:44 -07:00
|
|
|
}
|
2019-08-27 19:57:35 -07:00
|
|
|
startup_diff_time = current_time - startup_start_time;
|
2019-08-27 09:10:44 -07:00
|
|
|
|
|
|
|
// Calibration is done
|
2019-09-08 18:32:53 -07:00
|
|
|
if(ST2MS(startup_diff_time) > 1000){
|
2019-08-27 09:10:44 -07:00
|
|
|
// Set fault and wait for valid startup condition
|
|
|
|
state = FAULT;
|
2019-08-27 19:57:35 -07:00
|
|
|
startup_start_time = 0;
|
|
|
|
startup_diff_time = 0;
|
2019-08-27 09:10:44 -07:00
|
|
|
}
|
2019-08-04 11:47:23 -07:00
|
|
|
break;
|
|
|
|
case (RUNNING):
|
2019-08-07 00:22:51 -07:00
|
|
|
// Check for overspeed
|
2019-08-27 09:10:44 -07:00
|
|
|
if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){
|
2019-08-07 00:22:51 -07:00
|
|
|
state = DEAD;
|
|
|
|
}
|
|
|
|
|
2019-08-04 23:13:35 -07:00
|
|
|
// Check for fault
|
2019-09-08 18:32:53 -07:00
|
|
|
if(fabsf(m_angle) > balance_conf.m_fault || fabsf(c_angle) > balance_conf.c_fault){
|
2019-08-04 23:13:35 -07:00
|
|
|
state = FAULT;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Over speed tilt back safety
|
2019-08-27 09:10:44 -07:00
|
|
|
if(mc_interface_get_duty_cycle_now() > balance_conf.tiltback_duty){
|
|
|
|
setpoint_target = balance_conf.tiltback_angle;
|
2019-08-07 00:22:51 -07:00
|
|
|
setpointAdjustmentType = TILTBACK;
|
2019-08-27 09:10:44 -07:00
|
|
|
} else if(mc_interface_get_duty_cycle_now() < -balance_conf.tiltback_duty){
|
|
|
|
setpoint_target = -balance_conf.tiltback_angle;
|
2019-08-07 00:22:51 -07:00
|
|
|
setpointAdjustmentType = TILTBACK;
|
2019-08-04 23:13:35 -07:00
|
|
|
}else{
|
|
|
|
setpoint_target = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Adjust setpoint
|
|
|
|
if(setpoint != setpoint_target){
|
2019-08-07 00:22:51 -07:00
|
|
|
// If we are less than one step size away, go all the way
|
2019-08-10 00:16:48 -07:00
|
|
|
if(fabsf(setpoint_target - setpoint) < get_setpoint_adjustment_step_size()){
|
2019-08-07 00:22:51 -07:00
|
|
|
setpoint = setpoint_target;
|
|
|
|
}else if (setpoint_target - setpoint > 0){
|
2019-08-08 23:39:14 -07:00
|
|
|
setpoint += get_setpoint_adjustment_step_size();
|
2019-08-07 00:22:51 -07:00
|
|
|
}else{
|
2019-08-08 23:39:14 -07:00
|
|
|
setpoint -= get_setpoint_adjustment_step_size();
|
2019-08-04 23:13:35 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-08-04 11:47:23 -07:00
|
|
|
// Do PID maths
|
2019-09-08 18:32:53 -07:00
|
|
|
proportional = setpoint - m_angle;
|
2019-08-07 00:22:51 -07:00
|
|
|
// Apply deadzone
|
|
|
|
proportional = apply_deadzone(proportional);
|
|
|
|
// Resume real PID maths
|
2019-08-04 11:47:23 -07:00
|
|
|
integral = integral + proportional;
|
|
|
|
derivative = proportional - last_proportional;
|
|
|
|
|
2019-08-27 09:10:44 -07:00
|
|
|
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
|
2019-08-04 11:47:23 -07:00
|
|
|
|
|
|
|
last_proportional = proportional;
|
|
|
|
|
2019-08-07 00:22:51 -07:00
|
|
|
// Apply current boost
|
|
|
|
if(pid_value > 0){
|
2019-08-27 09:10:44 -07:00
|
|
|
pid_value += balance_conf.current_boost;
|
2019-08-07 00:22:51 -07:00
|
|
|
}else if(pid_value < 0){
|
2019-08-27 09:10:44 -07:00
|
|
|
pid_value -= balance_conf.current_boost;
|
2019-08-07 00:22:51 -07:00
|
|
|
}
|
|
|
|
|
2019-08-04 23:13:35 -07:00
|
|
|
// Reset the timeout
|
|
|
|
timeout_reset();
|
2019-08-04 11:47:23 -07:00
|
|
|
|
|
|
|
// Output to motor
|
2019-08-10 00:16:48 -07:00
|
|
|
if(pid_value == 0){
|
2019-08-09 23:16:20 -07:00
|
|
|
mc_interface_release_motor();
|
|
|
|
}else {
|
|
|
|
mc_interface_set_current(pid_value);
|
|
|
|
}
|
2019-08-04 11:47:23 -07:00
|
|
|
break;
|
|
|
|
case (FAULT):
|
2019-08-07 00:22:51 -07:00
|
|
|
// Check for valid startup position
|
2019-09-08 18:32:53 -07:00
|
|
|
if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance){
|
|
|
|
setpoint = m_angle;
|
2019-08-04 23:13:35 -07:00
|
|
|
setpoint_target = 0;
|
2019-08-27 19:57:35 -07:00
|
|
|
setpointAdjustmentType = CENTERING;
|
2019-08-04 23:13:35 -07:00
|
|
|
state = RUNNING;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2019-08-07 00:22:51 -07:00
|
|
|
// Disable output
|
|
|
|
mc_interface_set_current(0);
|
|
|
|
break;
|
|
|
|
case (DEAD):
|
2019-08-04 23:13:35 -07:00
|
|
|
// Disable output
|
|
|
|
mc_interface_set_current(0);
|
2019-08-04 11:47:23 -07:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Delay between loops
|
2019-08-27 09:10:44 -07:00
|
|
|
chThdSleepMicroseconds((int)((1000.0 / balance_conf.hertz) * 1000.0));
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
|
|
|
|
// Disable output
|
|
|
|
mc_interface_set_current(0);
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|