Shop OBEX P1 Docs P2 Docs Learn Events
Barely balancing — Parallax Forums

Barely balancing

ratronicratronic Posts: 1,451
edited 2011-11-08 06:37 in Robotics
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             
 

Comments

  • SapiehaSapieha Posts: 2,964
    edited 2011-08-30 16:29
    Hi ratronic.

    Nice work.
  • ercoerco Posts: 20,256
    edited 2011-08-30 17:07
    Sweet! You make it look easy!
  • Roy ElthamRoy Eltham Posts: 3,000
    edited 2011-08-30 17:23
    You probably want the IMU up higher.
  • graffixgraffix Posts: 389
    edited 2011-08-30 17:37
    Oh so jealous(kidding)very nice job though seems smooth.Shame Im not familiar with spin.
  • ratronicratronic Posts: 1,451
    edited 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!
  • graffixgraffix Posts: 389
    edited 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
  • ratronicratronic Posts: 1,451
    edited 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                             
    
    640 x 480 - 126K
  • graffixgraffix Posts: 389
    edited 2011-08-31 15:31
    Since you are using spin,have you seen Hannos dancebot code?
  • ratronicratronic Posts: 1,451
    edited 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.
  • ratronicratronic Posts: 1,451
    edited 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.
    1024 x 576 - 89K
Sign In or Register to comment.