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

balancing on a incline wooden board

velctrl
EmaMaker 2024-04-26 18:37:41 +02:00
parent 79cffdac4e
commit fcc8a4dafd
1 changed files with 13 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,12 +23,12 @@ constexpr double KI = KP * 10.8852;
constexpr double KD = 0.3; constexpr double KD = 0.3;
// PI constants for outer velocity control loop // PI constants for outer velocity control loop
constexpr double KP_vel = 0.0025; constexpr double KP_vel = 0.01;
constexpr double KI_vel = 0.0005; constexpr double KI_vel = 0.005;
float setpoint = 0.0; float angle_setpoint = 0.0;
float output = 0; float angle_output = 0;
float input = 0; float angle_input = 0;
float vel_setpoint = 0.0; float vel_setpoint = 0.0;
float vel_input = 0.0; float vel_input = 0.0;
@ -38,9 +38,9 @@ float yaw{ 0 }, pitch{ 0 }, roll{ 0 };
QuickPID pitchCtrl(&input, &output, &setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse); QuickPID pitchCtrl(&angle_input, &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(&output, &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::pOnMeas, velCtrl.dMode::dOnMeas, velCtrl.iAwMode::iAwCondition, velCtrl.Action::direct);
void setup() { void setup() {
Serial.begin(9600); Serial.begin(9600);
@ -55,7 +55,7 @@ void setup() {
// Let the initial error from madgwick filter discharge without affecting the integral term of the PID // Let the initial error from madgwick filter discharge without affecting the integral term of the PID
unsigned long t = millis(); unsigned long t = millis();
while (millis() - t < 5000) { while (millis() - t < 2000) {
update_imu(); update_imu();
} }
@ -64,7 +64,7 @@ void setup() {
pitchCtrl.SetMode(pitchCtrl.Control::automatic); pitchCtrl.SetMode(pitchCtrl.Control::automatic);
pitchCtrl.SetSampleTimeUs(1000); pitchCtrl.SetSampleTimeUs(1000);
velCtrl.SetOutputLimits(-0.15, 0.15); velCtrl.SetOutputLimits(-0.35, 0.35);
velCtrl.SetMode(velCtrl.Control::automatic); velCtrl.SetMode(velCtrl.Control::automatic);
velCtrl.SetSampleTimeUs(10000); velCtrl.SetSampleTimeUs(10000);
@ -122,22 +122,17 @@ void loop1(){
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();
input = pitch; angle_input = pitch;
Serial.println(setpoint);
// I also modified the ArduPID library to use compute as a boolean. If calculations were done, it returns true. If not enough time has elapsed, it returns false
if(pitchCtrl.Compute()){ if(pitchCtrl.Compute()){
double tvelocity = angle_output;
double tvelocity = output;
tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI; tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
frequency = tvelocity * 0.1125; frequency = tvelocity * 0.1125;