Barely balancing

ratronic · 2011-08-30 16:23

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

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

Sapieha · 2011-08-30 16:29

Hi ratronic.

Nice work.

erco · 2011-08-30 17:07

Sweet! You make it look easy!

Roy Eltham · 2011-08-30 17:23

You probably want the IMU up higher.

graffix · 2011-08-30 17:37

Oh so jealous(kidding)very nice job though seems smooth.Shame Im not familiar with spin.

ratronic · 2011-08-30 17:37

Thanks for that Roy. To be honest I didn't have a clue where to place it. (very new to this stuff) Do you think it has to be at the very top or can it sit in the middle on a proto board(properly positioned)? I am using the 5dof imu from sparkfun and the object for it in the obex.

Edit:Thank you everybody!

graffix · 2011-08-30 18:19

the higher up the better,its like balancing a broom stick vs. a pencil.things will move slower and give more time to react

ratronic · 2011-08-31 14:56

Ok I mounted the imu on top, but really see no difference. I've tried all kinds of different pid settings but all I can get it to do is oscillate back and forth. The imu object puts out nice clean output angles for pitch and roll but I think the output from the object is a little too delayed to use with a balancing bot. Here is the last code I tryed. If any of you mad scientific experts out there have any suggestions I would like to hear them.

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

graffix · 2011-08-31 15:31

Since you are using spin,have you seen Hannos dancebot code?

ratronic · 2011-08-31 16:01

Yes I've seen Hanno's. He is using fuzzy logic to balance that bot. The imu object in the OBEX I'm using uses a Kalman filter and was written by Jason Wood. I have glanced at Hanno's code before but I'm going to take a little closer look now. The imu object I am using doesn't seem to react fast enough even if I crank up the proportional gain.

ratronic · 2011-11-08 06:37

I looked at the output of the acclerometer (red trace) and the output of the same angle from the Kalman filter (black trace). The Kalman filter really cleans up the signal using the gryo to fuse the signals but using the IMU object from the OBEX there is ~.25 second delay between the acclerometer and Kalman filter which is I think is the main source of my problem. I have also changed the magnitude of the Kalman output as I need to sense changes of less than a degree. On to more experiments and try some more ! BTW the object I'm using is hidden in the math section of the OBEX under Kalman filtered imu.

Barely balancing

Comments