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_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
MPU6050 IMU; //Change to the name of any supported IMU!

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

@ -15,32 +15,45 @@ constexpr double MAX_HALF_PERIOD = 75; // in microseconds
// 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 CONVERSION_FACTOR = 180 / WHEEL_RADIUS / PI;
// 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
// I modified the ArduPID library to make it accept negative values for the parameters
constexpr double KP = 42;
constexpr double KI = KP * 10.8852;
constexpr double KD = 0.3;
constexpr float KP = 42;
constexpr float KI = KP * 10.8852;
constexpr float KD = 0.3;
// PI constants for outer velocity control loop
constexpr double KP_vel = 0.01;
constexpr double KI_vel = 0.005;
constexpr float KP_vel = 0.0065;
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_output = 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_input = 0.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
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() {
Serial.begin(9600);
@ -68,10 +81,16 @@ void setup() {
velCtrl.SetMode(velCtrl.Control::automatic);
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);
yaw_setpoint = yaw;
}
void setup1(){
void setup1() {
pinMode(MOT_DX_DIR, OUTPUT);
pinMode(MOT_SX_DIR, OUTPUT);
@ -79,43 +98,53 @@ void setup1(){
pinMode(MOT_SX_STEP, OUTPUT);
}
unsigned long last_time_motors = micros(), current_time_motors = micros();
bool b = true;
// Such a long halfperiod means that the motors are not moving
uint32_t t = INT_MAX;
int32_t period = INT_MAX, halfperiod1 = INT_MAX;
void loop1(){
void loop1() {
static bool sx_b = true, dx_b = true;
static uint32_t last_time_sx = micros(), last_time_dx = micros(), current_time_motors = micros();
// 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
if(rp2040.fifo.available()) {
rp2040.fifo.pop_nb(&t);
int32_t period = (int32_t)t;
if (rp2040.fifo.available() > 2) {
uint32_t t;
rp2040.fifo.pop_nb(&t);
tmotsx_period = (int32_t) t;
rp2040.fifo.pop_nb(&t);
tmotdx_period = (int32_t) t;
}
// Direction need to be changed during motor pulse
// Doing it here unsure it happens at the correct time
if(period > 0){
// Positivie direction
digitalWriteFast(MOT_DX_DIR, HIGH);
if (tmotsx_period > 0) {
digitalWriteFast(MOT_SX_DIR, HIGH);
halfperiod1 = (uint32_t)(period);
}else{
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);
digitalWriteFast(MOT_SX_DIR, LOW);
halfperiod1 = (uint32_t)(-period);
}
motdx_period = (uint32_t)(-tmotdx_period);
}
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
digitalWriteFast(MOT_DX_STEP, b);
digitalWriteFast(MOT_SX_STEP, b);
b = !b;
last_time_motors = current_time_motors;
digitalWriteFast(MOT_SX_STEP, sx_b);
sx_b = !sx_b;
last_time_sx = 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;
}
}
@ -129,17 +158,19 @@ void loop() {
update_imu();
velCtrl.Compute();
yawCtrl.Compute();
angle_input = pitch;
if(pitchCtrl.Compute()){
if (pitchCtrl.Compute()) {
double tvelocity = angle_output;
tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
frequency = tvelocity * 0.1125;
half_period0 = 1000000 / (2*frequency);
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);
// 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 the other motor to core 1. Non blocking
rp2040.fifo.push_nb(half_period0);
}
}