Balancing Bot using a Comp Filter
I have been wanting to build a balancing bot for awhile now. Actually been working on the code for a long time. I finally have the bot built and balancing, so I thought I would share a short video.
All the bot can currently do is balance, but I was pretty excited last night when I fired it up for the first time and it kind of stayed in one place! The video is after about 20 minutes of tuning the PID loop.
The video is pretty boring, hopefully as I progress I will be able to post more interesting ones.
https://vimeo.com/396792477
Shawn A.
All the bot can currently do is balance, but I was pretty excited last night when I fired it up for the first time and it kind of stayed in one place! The video is after about 20 minutes of tuning the PID loop.
The video is pretty boring, hopefully as I progress I will be able to post more interesting ones.
https://vimeo.com/396792477
Shawn A.
Comments
I'm betting steppers.
I absolutely agree. Fantastic robot Shawna!
Here's hoping the figure 8 thread gets a new entry.
Thanks for sharing this with us.
I have been trying to refine my stepper motor code so that it starts and stop more smoothly and for fast accelerations does not stall. I'm using 16 step mode.
Mike
dgately
They are stepper motors. I am more comfortable with stepper motors, but dc motors with encoders may have been a better route to take. I am having problems working out how to keep track of the forwards and backwards movement or travel. I know the theory of how to do keep track with steppers, but I am not sure how to code it.
Duane,
Ercos-Figure-Eight-Challenge is one of my goals.
I was worried about the stepper motors stalling when they changed direction, but they don't. The only time the motors stall, is when the integral winds up too fast. This can be stopped by clamping the integral term so it doesn't run away. I haven't done this yet, but need too!
I am using most of the same parts as your Roadster. forums.parallax.com/discussion/169968/dual-overhead-a4988-roadster#latest
2 - A4988 stepper motor drivers
2 - 80x10mm wheels
2 - Nema 17 stepper motors 1.7A. 1.7A is probably over kill and would be hard on the drivers at their rated current, I think.
1 - 4S Lipo 2200mah battery.
1 - Parallax Activity Board AX
1 - Parallax WX ESP8266 WiFi Module Dip
The code is written using SimpleIDE.
Here is my stepper motor code which runs in its own processor(Cog). Pretty basic right now, it can only balance. Need to add turning and movement tracking to it. The PidOutput variable comes from another processor dedicated to the PID code. It may be possible down the road to combine the PID code and the Stepper code into a single processor. The PID code is basically copied from the learn site example here. learn.parallax.com/tutorials/language/pbasic/pid-control/proportional-integral-and-derivative
The setpoint for the PID is an angle and sensor input is the angle estimated from the Comp Filter. The output of the Comp Filter is an Integer and the PID math is all done in Integers.
void StepperDriver(void *par) { int left_motor_step_pin = 17; int right_motor_step_pin = 15; int left_motor_dir_pin = 16; int right_motor_dir_pin = 14; int p_PidOutput = 0; set_directions(17, 14, 0b1111); //set_directions(15, 17, 0b101); //CTRA = (0b00100 << 26) + 17; //CTRB = (0b00100 << 26) + 15; int loop_sync = CNT; while(1) { // Used for Timing. start = CNT; // Transfer global varibles to local. p_PidOutput = PidOut ; // If Else statement added too shut down motors during start up or if the Bot tips over. if(startPid == 0) { CTRA = 0; CTRB = 0; } else { CTRA = (0b00100 << 26) + 17; CTRB = (0b00100 << 26) + 15; } // Forwards and Backwards Code. if (p_PidOutput < 0) { p_PidOutput = p_PidOutput * -1; low(16); high(14); FRQA = p_PidOutput; FRQB = p_PidOutput; } else if(p_PidOutput > 0) { high(16); low(14); FRQA = p_PidOutput; FRQB = p_PidOutput; } // Used for Timing. stop = CNT; timeUs = stop - start; waitcnt(loop_sync + (CLKFREQ / 100)); // 100Hz loop, 10mS. loop_sync = CNT; } } /*-------------------------------------------------------------------------------------*/Edit: The code above has a few things in it that need too be cleaned up to make it more efficient, but it works fine.
/*-------------------------------------------------------------------------------------*/ void PID_Control(void *par) { int p = 0; int i = 0; int d = 0; int Pitch_Kp = 500; //300, 350, 300, 300, 350 int Pitch_Ki = 50; //100, 100, 20, 50, 50 int Pitch_Kd = 75; //20, 40, 50, 50, 100 int Pitch_Current_Error = 0; int Pitch_Accumulated_Error = 0; int Pitch_Previous_Error = 0; int Pitch_Error_Delta = 0; int Pitch_error = 0; int Pitch_PID_Sensor_Input = 0; int Pitch_PID_Setpoint = -50; int Pitch_PID_Output = 0; pause(300); cog_run(&StepperDriver, 128); // Start stepper motor driver. int loop_sync = CNT; while(1) { //start = CNT; // Transfer global varibles to local. Pitch_PID_Sensor_Input = Pitch / 10; //Testing to try and dampen noise in the PID Pitch_PID_Sensor_Input = Pitch_PID_Sensor_Input * 10; //Testing to try and dampen noise in the PID // Waits to start Pid until bot has been stood up. while(startPid == 0) { if((Pitch < 100) && (Pitch > -100))startPid = 1; //Start Pid once angle is between 1 and -1 degrees. loop_sync = CNT; } // Calculate Error. Pitch_Current_Error = Pitch_PID_Setpoint - Pitch_PID_Sensor_Input; // Calculate proportional term. p = Pitch_Kp * Pitch_Current_Error; // Calculate integral term. Pitch_Accumulated_Error = Pitch_Accumulated_Error + Pitch_Current_Error; i = Pitch_Ki * Pitch_Accumulated_Error; // Calculate derivative term. Pitch_Error_Delta = Pitch_Current_Error - Pitch_Previous_Error; d = Pitch_Kd * Pitch_Error_Delta; // Calculate PID Output. Pitch_PID_Output = p + i + d; // Save current error as Previous. Pitch_Previous_Error = Pitch_Current_Error; if((Pitch_PID_Sensor_Input > 2000) || (Pitch_PID_Sensor_Input < -2000)) // Turn motors off if Bot has tipped more than 20degrees. { Pitch_PID_Output = 0; startPid = 0; Pitch_Accumulated_Error = 0; } // Transfer local variables to global variables. PidOut = Pitch_PID_Output; //stop = CNT; //timeUs = stop - start; waitcnt(loop_sync + (CLKFREQ / 100)); // 100Hz loop, 10mS. loop_sync = CNT; } } /*-------------------------------------------------------------------------------------*/It's based on 10 hz per unit so 1 = 10 pulses per second up to about 256.
The balancing robot that I say used two pids. One for balance and another for movement where it was only a PI. It would input the speed forward or backwards with left and right speeds.
Anyway here is my stepper library with documentation.
Mike
Mike thanks for the code. I am looking at your code and I like what you've done, and I have a few comments.
I like what you have done with basing each increment to a know hz value. If I am not mistaken that makes the stepper motor response linear, which I think is desirable.
I am still trying to figure out how you came up the multiplier. Currently digging through the AN001 Counter data sheet.
The multiplier in your library is 5369. I think that comes up with a minimum frequency of 100Hz not 10Hz. If my math is right, that maybe why you where having problems with the bot balancing and the motors stalling.
(5369 / 2^32) * 80,000,000 = 100.00541
I maybe missing something here.
If your PID ouput is 0 the frequency would be 0hz. Then if the PID output changed to 1 the frequency would be 100hz. That's a pretty drastic change I think!
My stepper motor is 1.8 degrees/step. At 16x steps that's 3200 steps per revolution. 360 / 3200 = .1125
.1125 would be degrees rotate per step at 16x step.
So at 100hz scale an output of 1 would rotate the motor 11.25 degrees in 1 second. If the scale was 10hz it would be 1.125 degrees of rotation in 1 second.
I maybe completely off point here!
I'm thinking about trying something like 1hz as a minimum.
Shawn
Here is the section of code from Mikes stepper driver.
void Stepper_speed(short speedA, short speedB) { int A, B; A = speedA * 5369; // 80Mhz to even 10Hz B = speedB * 5369; FRQA = A; FRQB = B; // printi("Frequency: %d\n", S); }@"Duane Degn" : Very nice to hear you weighing in, also plugging the figure 8 challenge. That reminds me that I never tried a fig 8 with my ultra-simple BS-2 balancer... have to dig that out and give it a shot sometime soon.