EmaMaker
/
Two-Wheel-Self-Balancing-Robot-v2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial yaw ctrl for turning 

gyro drift hits hard
yawctrl
EmaMaker 2024-04-27 11:47:17 +02:00
parent d1cb40d10c
commit 36a745243c
2 changed files with 83 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
selfbalance-madgwick/imu.ino
View File

@ -4,7 +4,7 @@
//#define PRINT_ANGLES //#define PRINT_ANGLES
//#define PRINT_QUAT //#define PRINT_QUAT
#define PERFORM_CALIBRATION //Comment to disable startup calibration // #define PERFORM_CALIBRATION //Comment to disable startup calibration
#define IMU_ADDRESS 0x68 //Change to the address of the IMU #define IMU_ADDRESS 0x68 //Change to the address of the IMU
MPU6050 IMU; //Change to the name of any supported IMU! MPU6050 IMU; //Change to the name of any supported IMU!

133
selfbalance-madgwick/selfbalance-madgwick.ino
View File

@ -11,36 +11,49 @@ constexpr double ANGLE_PER_STEP = 0.1125;
constexpr double WEIGHT = 0.961; constexpr double WEIGHT = 0.961;
constexpr double WHEEL_RADIUS = 0.0475; constexpr double WHEEL_RADIUS = 0.0475;
// Experimentally, the lowest pulse my steppers can handle without stalling + some leeway // Experimentally, the lowest pulse my steppers can handle without stalling + some leeway
constexpr double MAX_HALF_PERIOD = 75; // in microseconds constexpr double MAX_HALF_PERIOD = 75; // in microseconds
// Which means there is a maximum velocity achievable // Which means there is a maximum velocity achievable
constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD * ANGLE_PER_STEP) * PI / 180; constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD * ANGLE_PER_STEP) * PI / 180;
constexpr double CONVERSION_FACTOR = 180 / WHEEL_RADIUS / PI;
// Derived and analytical model, linearized it and simulated in MATLAB. // Derived and analytical model, linearized it and simulated in MATLAB.
// PID values are then calculated and verified by simulation in Simulink. I ain't calibrating a PID by hand on this robot // PID values are then calculated and verified by simulation in Simulink. I ain't calibrating a PID by hand on this robot
// I modified the ArduPID library to make it accept negative values for the parameters // I modified the ArduPID library to make it accept negative values for the parameters
constexpr double KP = 42; constexpr float KP = 42;
constexpr double KI = KP * 10.8852; constexpr float KI = KP * 10.8852;
constexpr double KD = 0.3; constexpr float KD = 0.3;
// PI constants for outer velocity control loop // PI constants for outer velocity control loop
constexpr double KP_vel = 0.01; constexpr float KP_vel = 0.0065;
constexpr double KI_vel = 0.005; constexpr float KI_vel = 0.0005;
constexpr float KP_yaw = 0.8;
constexpr float KI_yaw = 0.06;
float angle_setpoint = 0.0; float angle_setpoint = 0.0;
float angle_output = 0; float angle_output = 0;
float angle_input = 0; float angle_input = 0;
float yaw_setpoint = 0.0;
float yaw_output = 0;
float yaw_input = 0;
float vel_setpoint = 0.0; float vel_setpoint = 0.0;
float vel_input = 0.0; float vel_input = 0.0;
float yaw{ 0 }, pitch{ 0 }, roll{ 0 }; float yaw{ 0 }, pitch{ 0 }, roll{ 0 };
QuickPID pitchCtrl(&angle_input, &angle_output, &angle_setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse); QuickPID pitchCtrl(&pitch, &angle_output, &angle_setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse);
// TODO: a little deadzone when the input is almost 0, just to avoid unnecessary "nervous" control and eventual oscillations // TODO: a little deadzone when the input is almost 0, just to avoid unnecessary "nervous" control and eventual oscillations
QuickPID velCtrl(&angle_output, &angle_setpoint, &vel_setpoint, KP_vel, KI_vel, 0, velCtrl.pMode::pOnMeas, velCtrl.dMode::dOnMeas, velCtrl.iAwMode::iAwCondition, velCtrl.Action::direct); QuickPID velCtrl(&angle_output, &angle_setpoint, &vel_setpoint, KP_vel, KI_vel, 0, velCtrl.pMode::pOnError, velCtrl.dMode::dOnMeas, velCtrl.iAwMode::iAwClamp, velCtrl.Action::direct);
QuickPID yawCtrl(&yaw, &yaw_output, &yaw_setpoint, KP_yaw, KI_yaw, 0, yawCtrl.pMode::pOnError, yawCtrl.dMode::dOnMeas, yawCtrl.iAwMode::iAwClamp, yawCtrl.Action::direct);
void setup() { void setup() {
Serial.begin(9600); Serial.begin(9600);
@ -48,7 +61,7 @@ void setup() {
delay(1000); delay(1000);
setup_imu(); setup_imu();
// Just to signal it is working // Just to signal it is working
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH); digitalWrite(LED_BUILTIN, HIGH);
@ -68,78 +81,96 @@ void setup() {
velCtrl.SetMode(velCtrl.Control::automatic); velCtrl.SetMode(velCtrl.Control::automatic);
velCtrl.SetSampleTimeUs(10000); velCtrl.SetSampleTimeUs(10000);
yawCtrl.SetOutputLimits(-MAX_VELOCITY*0.01, MAX_VELOCITY*0.01);
yawCtrl.SetMode(yawCtrl.Control::automatic);
yawCtrl.SetSampleTimeUs(10000);
digitalWrite(LED_BUILTIN, LOW); digitalWrite(LED_BUILTIN, LOW);
yaw_setpoint = yaw;
} }
void setup1(){ void setup1() {
pinMode(MOT_DX_DIR, OUTPUT); pinMode(MOT_DX_DIR, OUTPUT);
pinMode(MOT_SX_DIR, OUTPUT); pinMode(MOT_SX_DIR, OUTPUT);
pinMode(MOT_DX_STEP, OUTPUT); pinMode(MOT_DX_STEP, OUTPUT);
pinMode(MOT_SX_STEP, OUTPUT); pinMode(MOT_SX_STEP, OUTPUT);
} }
unsigned long last_time_motors = micros(), current_time_motors = micros(); void loop1() {
bool b = true; static bool sx_b = true, dx_b = true;
// Such a long halfperiod means that the motors are not moving static uint32_t last_time_sx = micros(), last_time_dx = micros(), current_time_motors = micros();
uint32_t t = INT_MAX;
int32_t period = INT_MAX, halfperiod1 = INT_MAX;
void loop1(){
// TODO: handle the steppers using interrupt timers. The second core could be used for IMU processing, while the first one handles the different control loops // TODO: handle the steppers using interrupt timers. The second core could be used for IMU processing, while the first one handles the different control loops
static int32_t tmotsx_period = INT_MAX, tmotdx_period = INT_MAX;
static uint32_t motsx_period = INT_MAX, motdx_period = INT_MAX;
// Retrieve the half period value from core0. Non blocking call // Retrieve the half period value from core0. Non blocking call
if(rp2040.fifo.available()) {
rp2040.fifo.pop_nb(&t);
int32_t period = (int32_t)t;
// Direction need to be changed during motor pulse if (rp2040.fifo.available() > 2) {
// Doing it here unsure it happens at the correct time uint32_t t;
if(period > 0){ rp2040.fifo.pop_nb(&t);
// Positivie direction tmotsx_period = (int32_t) t;
digitalWriteFast(MOT_DX_DIR, HIGH); rp2040.fifo.pop_nb(&t);
digitalWriteFast(MOT_SX_DIR, HIGH); tmotdx_period = (int32_t) t;
halfperiod1 = (uint32_t)(period); }
}else{
// Negative direction // Direction need to be changed during motor pulse
digitalWriteFast(MOT_DX_DIR, LOW); // Doing it here unsure it happens at the correct time
digitalWriteFast(MOT_SX_DIR, LOW); if (tmotsx_period > 0) {
halfperiod1 = (uint32_t)(-period); digitalWriteFast(MOT_SX_DIR, HIGH);
} motsx_period = (uint32_t)(tmotsx_period);
} else {
// Negative direction
digitalWriteFast(MOT_SX_DIR, LOW);
motsx_period = (uint32_t)(-tmotsx_period);
}
if (tmotdx_period > 0) {
digitalWriteFast(MOT_DX_DIR, HIGH);
motdx_period = (uint32_t)(tmotdx_period);
} else {
// Negative direction
digitalWriteFast(MOT_DX_DIR, LOW);
motdx_period = (uint32_t)(-tmotdx_period);
} }
current_time_motors = micros(); current_time_motors = micros();
if(current_time_motors - last_time_motors > halfperiod1){ if (current_time_motors - last_time_sx > motsx_period) {
// Half a pulse. Next cycle will be the rest // Half a pulse. Next cycle will be the rest
digitalWriteFast(MOT_DX_STEP, b); digitalWriteFast(MOT_SX_STEP, sx_b);
digitalWriteFast(MOT_SX_STEP, b); sx_b = !sx_b;
b = !b; last_time_sx = current_time_motors;
last_time_motors = current_time_motors; }
if (current_time_motors - last_time_dx > motdx_period) {
// Half a pulse. Next cycle will be the rest
digitalWriteFast(MOT_DX_STEP, dx_b);
dx_b = !dx_b;
last_time_dx = current_time_motors;
} }
} }
double frequency = 0; double frequency = 0;
int32_t half_period0 = 0; int32_t half_period0 = 0;
double velocity = 0; double velocity = 0;
uint32_t current_time = millis(), last_time = millis(); uint32_t current_time = millis(), last_time = millis();
void loop() { void loop() {
update_imu(); update_imu();
velCtrl.Compute(); velCtrl.Compute();
yawCtrl.Compute();
angle_input = pitch; if (pitchCtrl.Compute()) {
if(pitchCtrl.Compute()){
double tvelocity = angle_output; double tvelocity = angle_output;
tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
frequency = tvelocity * 0.1125;
half_period0 = 1000000 / (2*frequency);
// Send the half period to core 1. Non blocking frequency = (tvelocity + yaw_output) * CONVERSION_FACTOR * ANGLE_PER_STEP ;
half_period0 = 1000000 / (2 * frequency);
// Send the half period of one motor to core 1. Non blocking
rp2040.fifo.push_nb(half_period0);
frequency = (tvelocity - yaw_output) * CONVERSION_FACTOR * ANGLE_PER_STEP ;
half_period0 = 1000000 / (2 * frequency);
// Send the half period of the other motor to core 1. Non blocking
rp2040.fifo.push_nb(half_period0); rp2040.fifo.push_nb(half_period0);
} }
} }