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

balance PID: update input before computing

main
EmaMaker 2024-04-03 20:49:41 +02:00
parent df72e84e54
commit b9b03fcdb4
1 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,7 +54,7 @@ void setup() {
} }
pitchCtrl.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD); pitchCtrl.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
pitchCtrl.setOutputLimits(-MAX_VELOCITY, MAX_VELOCITY); // double of max torque motors can exhert pitchCtrl.setOutputLimits(-MAX_VELOCITY, MAX_VELOCITY);
//pitchCtrl.setWindUpLimits(-10, 10); //pitchCtrl.setWindUpLimits(-10, 10);
pitchCtrl.setSampleTime(1); pitchCtrl.setSampleTime(1);
pitchCtrl.start(); pitchCtrl.start();
@ -88,19 +88,17 @@ void loop1(){
} }
} }
unsigned long last_time = millis(), current_time = millis(), time_diff;
double frequency = 0; double frequency = 0;
unsigned long half_period0 = 0; unsigned long half_period0 = 0;
double velocity = 0; double velocity = 0;
void loop() { void loop() {
current_time = millis();
time_diff = current_time - last_time;
update_imu(); update_imu();
input = pitch;
// 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 // 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()){
input = pitch;
// Keeping it here // Keeping it here
/* /*
@ -139,8 +137,6 @@ void loop() {
Serial.print(frequency); Serial.print(frequency);
Serial.print(" | "); Serial.print(" | ");
Serial.println(half_period0);*/ Serial.println(half_period0);*/
last_time = current_time;
} }
} }