Balance-bot + PID

Stoshic

I am trying to build a balance-bot using J. Wood’s IMU/Kalman filter program (which works great) and a PID loop. I have experimented with just using a P loop and a PD loop without anything close to success. My problem is that no matter how I set Kp and/or Kd, my bot goes into oscillations of increasing amplitude. I have no prior experience with PID loops but I have read available material and constructed mine based on what I have read.

The balance bot hardware consists of a 5-DOF IMU mounted near the battery at the top of the bot. The motors and wheels are from the Parallax 7.2 V motor, bracket and wheel kit along with 2 HB-25 motor controllers. The processor is the propeller mounted on the Propeller USB proto board.

My PID software is listed below. I would greatly appreciate any help that can be offered. Thanks.




CON

_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000


CS = 20
DIO = 19
CLK = 18

CLS = 16
CR = 13
Kp = 8.0
Kd =1.5
ki =0.0
low_offset = 640
high_offset = 700
setpoint = 86.0

OBJ

IMU : "IMU"
debug : "FullDuplexSerialPlus"
fstring : "floatstring"
fMath : "Float32Full"

Var

long current_angle
long speed
long P_correction
long t
long total_correction
long angleDec
long angleRound
long accumulated_error
long I_correction
long prev_error
long error
long D_correction



PUB main

debug.start(31, 30, 0, 57600)
waitcnt(clkfreq*2 + cnt)
debug.tx(CLS)

IMU.start (CS, DIO, CLK)
fmath.start



ctra[noparse][[/noparse]30..26] := %00100 ' Configure Counter A to NCO
ctra[noparse][[/noparse]5..0] := 5 ' Output pin 5
frqa := 1
dira~~
t := cnt


repeat
current_angle := imu.get_angle_Roll 'get current angle fom IMU

error := (fmath.fsub(setpoint, current_angle )) 'determine error
P_correction := (fmath.fmul(Kp,error)) 'determine P correction

D_correction := (fmath.fmul(Kd,(fmath.fsub(error, prev_error)))) 'determine D correction
prev_error := error

{{
accumulated_error := (fmath.fadd(error, accumulated_error)) 'I AM CURRENTLY NOT USING THE I
I_correction := (fmath.fmul(Ki,accumulated_error)) <# 100.0 'determine I correction with max
}}

total_correction := (fmath.fadd(fmath.fadd(P_correction, D_correction), I_correction))

debug.str(fstring.floattostring(current_angle ))
debug.str(string(" "))
debug.str(fstring.floattostring(error ))
debug.str(string(" "))
debug.str(fstring.floattostring(P_correction ))
debug.str(string(" "))
debug.str(fstring.floattostring(D_correction ))
debug.str(string(" "))
debug.str(fstring.floattostring(total_correction ))
debug.str(string(" "))
debug.tx(CR)


compute_speed

run_motors


pub compute_speed

angleDec := fmath.fround(total_correction)
if (angleDec < 0) 'I have tried different windows besides 0 such as +/- 1 through +/- 4
speed := clkfreq/(angleDec+low_offset) <# 160000 'max speed back = 1ms
elseif (angleDec > 0) 'offsets are values where wheels start to spin
speed := clkfreq/(angleDec+high_offset) #> 80000 'max speed fwd = 2ms
else
speed := clkfreq/665

pub run_motors

phsa := - speed ' Send the countdown pulse (speed)

{{
t += clkfreq/100 ' calculate delay between pulses
waitcnt(t) ' THE DELAY BETWEN PULSES IS CURRENTLY COMMENTED OUT since debug statements
' create ~ 10ms wait between pulses
}}

Mike Green

If you want anyone to pay attention to your posting, you need to use the Attachment Manager which you get when you use the Post Reply button. You need to attach your source code as a file, not cut and paste it. It's possible to use the [noparse][[/noparse] code ] and [noparse][[/noparse] /code ] tags (without the extra spaces), but the forum software still uses some things inside square brackets [noparse][[/noparse] ] as control markers. That's what happened to your program.

People generally are not too interested when someone just posts a large block of code or a whole complex program. It's better if you ask specific questions rather than "what do you think of this?" or "this doesn't work, what do I do?". In your case, you did describe what's going on, but the forum formatting messed things up. Since you have debugging statements in your program, you might give some examples of the values shown as your program "misbehaves".

Post Edited (Mike Green) : 7/14/2010 8:54:42 PM GMT

Stoshic

OK. Sorry about that. Here is the code as an attachment. My specific questions concern the equations for the PID loop. Is my approach correct? Can you see a reason why I just get increasing oscillations, no matter how I set Kp and/or Kd?

Timmoore

Some general comments
1. Normally in a pid the Kd term uses time, if you are very sure the delays are constant you can bundle the time into the constant but it is often better to div the error term by the time from the last tick before multipying by Kd.
2. Are you sure the kalman filter is working correctly. I have had cases where the axises are wrong (e.g. accelerometer X axis and gyro Y axis) and the filter looks like its working but was tracking slowly because it was really only working off the accelerometer - the effect was the feedback response was slow
3. Generally when tuning a PID, you set Ki and Kd to 0, then start with Kp small and increase until you get oscillation and then decrease Kp until oscillation stops and then tune the other constants
4. The setpoint is 86.0 I assume this happens to be the upright angle for your bot

Lawson

Have you tried a negative value for Kp and/or Kd? It sure sounds like your control loop is unconditionally unstable, likely due to using positive feedback when negative feedback is needed.

Lawson

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Lunch cures all problems! have you had lunch?

John Abshier

I don't see anything wrong in the PID calculation. I don't understand compute_speed. I would debug speed.

John Abshier

Stoshic

Thanks to everyone for the suggestions.

I tried both negative a positive Kp/Kd's without success. I think that the Kalman filter is working since the change in angle seems to be very fast and sensitive when I read it on the terminal. I will try dividing the error term by time and see if it makes a difference. The setpoint of 86.0 is the angle that the bot is perfectly balanced upright.

I put offsets (forward and back) into the compute_speed routine because there is a "dead zone" where the speed values do not provide enough motor torque for the wheels to turn given the weight
of the bot (about 3 pounds). The if-then statement directs the correction to be applied to the proper offset. The min/max limits keep the speed value within the limits of the motor controller. I have debugged speed and the values seem in line with what I would expect, but I will look again.

I'll keep working on it. Any additional suggestions are more than welcome!

Timmoore

You could be right but I would plot the data to be sure, I had a case of the wrong axis and looking through the printed data wasnt at all obvious what was happening. Plotting it made it obvious - the IMU output was changing in ~1/4 second but lagged the sensors by that amount of time.

You need to print the accelerometer and gyro sensor inputs as well as the imu output and compare the times of when they change compared to the imu output.

I have attached a plot of when I did it. The top line is the imu output, you can see the delay - its ~1/4sec. The other 2 lines are the gyro and acc· raw from the sensors. The gyro being the short spikes, you can see the gyro returning to 0 before the imu really changes, 1/4 sec doesn't seem a lot but it was enough that it meant the imu wasn't keeping the quadcopter stable.

Even just printing the sensor data would help, print it and then rotate round 1 axis and check the expected sensors are changing.

simonl

<OT>
@Tommore: Lovin' your graphing - any chance of releasing that software?
</OT>

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon

---

www.norfolkhelicopterclub.com

Announcement: To cut costs in the current economic climate, we have switched-off the light at the end of the tunnel.

Timmoore

@Simonl, Hope to soon.

I didn't re-run the test, I had the log file saved and loaded it into the config app and it loads it in as if it just did the run. The config settings are set correctly, the chart loaded, etc. Takes some time to load depending on the size of the file since that wasn't the goal of the app but it works and is surprising useful.

Post Edited (Timmoore) : 7/16/2010 2:31:42 AM GMT

nomad

Ref.@Timmore "Your QuadCopter-Interface...at the thread "Balance-bot + PID"

hi Timmore,
at your answer from 07-15-2010 i see the screenshot of your Quadcopter-Interface
it's looks cool
its possible to share your source-code ????
i am working on my quadcopter
in the moment i working on a 3D-Simulations of a quadcopter
with wings3d, opengl, OpenSceneGraph, ODE
and i am verry interessting on your source-code....
many thanks...
regards nomad

Timmoore

There is a release of the software posted on the Quad thread. I have a later version buts its still in development. I have been busy at work and haven't had much time to work on it. I can post the current state which has more functionality but its not all tested yet.

nomad

hi timmore
excuse me for the late answer. thanks for your code
(at quadcopterThread page 21)
regards nomad