Barely balancing
This is the chassis I bought off another thread. I have spent the last several days trying to get it to balance with an imu but I still have a ways to go. So instead I've attached a ping to the front bottom (I know a broken ping in waiting). You can probably see the imu mounted on the bottom plate and all of the cabling hanging around. Using the ping I hardly had to tune the pid (you can propably tell by the numbers). It was a fun time out, now back to the imu.
http://www.youtube.com/watch?v=zeyZAhsozUI
http://www.youtube.com/watch?v=zeyZAhsozUI
Con
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
sp = 23350.0
pwm = 0
Obj
pd : "pid"
pw : "pwmx8"
Var
long a, b
Pub main
pw.start(pwm,%00001111,10000)
''kp, ki, kd, setpoint, offset, max area, min area
pd.init(0.9, 0.9, 0.09, sp, 0.0, 0.0, 0.0)
repeat
a := ticks(7)
b := pd.calculate(a)
motors(b, b-12)
waitcnt(clkfreq/700 + cnt)
Pub motors(left, right) ''set the speed of the left and right wheel motors, -255 max.reverse to 255 max.forward, 0=stop
left := -255 #> left <# 255 ''limit left motor speed from -255 to 255
right := -255 #> right <# 255 ''limit right motor speed from -255 to 255
ifnot left & $80_00_00_00 ''check sign bit for left motor speed amount
pw.duty(pwm, 0) ''set pwm output for pwm base pin to 0
pw.duty(pwm + 1, left & $ff) ''set pwm output for left base pin + 1 limited to 255
else
pw.duty(pwm, (0 - left) & $ff) ''set pwm output for left base pin limited to 255
pw.duty(pwm + 1, 0) ''set pwm output for pwm base pin + 1 to 0
ifnot right & $80_00_00_00 ''check sign bit for right motor speed amount
pw.duty(pwm + 2, right & $ff) ''set pwm output for right base pin + 2 limited to 255
pw.duty(pwm + 3, 0) ''set pwm output for right base pin + 3 to 0
else
pw.duty(pwm + 2, 0) ''set pwm output for right base pin + 2 to 0
pw.duty(pwm + 3, (0 - right) & $ff) ''set pwm output for right base pin + 3 limited to 255
Pub ticks(pin)| cnt1, cnt2
''Return Ping)))'s full ultrasonic travel time in clockticks
outa[pin]~ ' Clear I/O Pin
dira[pin]~~ ' Make Pin Output
outa[pin]~~ ' Set I/O Pin
outa[pin]~ ' Clear I/O Pin (> 2 µs pulse)
dira[pin]~ ' Make I/O Pin Input
waitpne(0, |< pin, 0) ' Wait For Pin To Go HIGH
cnt1 := cnt ' Store Current Counter Value
waitpeq(0, |< pin, 0) ' Wait For Pin To Go LOW
cnt2 := cnt ' Store New Counter Value
return ||(cnt1-cnt2) ' Return Ping's full time of travel in clockticks
Comments
Nice work.
Edit:Thank you everybody!
Con _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 pwm = 0 ''pwm base pin sp = -7.5 ''pid setpoint Var long a,b,c,d Obj pd : "pid" pw : "pwmx8" im : "imu" fm : "floatmath" Pub main pw.start(pwm,%00001111,10000) im.start(7,6,5) ''kp, ki, kd, setpoint, offset, max area, min area pd.init(90.0, 9.0, 0.9, sp, 0.0, 0.0, 0.0) repeat a := im.get_angle_pitch ''get angle pitch in floating point degrees b := pd.calculate(a) ''pid motor calculation (floating point in - rounded integer out) motors(b, b) ''apply pid calculation to motors Pub motors(left, right) ''set the speed of the left and right wheel motors, -255 max.reverse to 255 max.forward, 0=stop left := -255 #> left <# 255 ''limit left motor speed from -255 to 255 right := -255 #> right <# 255 ''limit right motor speed from -255 to 255 ifnot left & $80_00_00_00 ''check sign bit for left motor speed amount pw.duty(pwm + 1, left & $ff) ''set pwm output for left base pin + 1 limited to 255 pw.duty(pwm, 0) ''set pwm output for pwm base pin to 0 else pw.duty(pwm + 1, 0) ''set pwm output for pwm base pin + 1 to 0 pw.duty(pwm, (0 - left) & $ff) ''set pwm output for left base pin limited to 255 ifnot right & $80_00_00_00 ''check sign bit for right motor speed amount pw.duty(pwm + 2, right & $ff) ''set pwm output for right base pin + 2 limited to 255 pw.duty(pwm + 3, 0) ''set pwm output for right base pin + 3 to 0 else pw.duty(pwm + 2, 0) ''set pwm output for right base pin + 2 to 0 pw.duty(pwm + 3, (0 - right) & $ff) ''set pwm output for right base pin + 3 limited to 255