Stingray Ping PID follower

ratronic

This is a Stingray PID follower demo using the dual pings setup (thanks Tony B.!) for left and right distance measuring. This demo shows the Stingray following an empty Parallax shipping box @ 10 inches.
For anybody that setup the pings on their Stingray like Tony B. showed in this thread- http://forums.parallax.com/showthread.php?123541-New-Take-on-Pings-for-the-Stingray give this a try. (setting up the proper i/o pins for the pings and their servos in the Con section for your robot)

So far I have added this ping setup, set of motor encoders, ir receiver, rf transceiver, network camara, an arm/gripper, and a 7.2volt 5000mah LiPo battery to my Stingray. All you need though to try this demo
is the Stingray and the ping setup. Download the stingray pid follower.zip file, unzip and click on stingray pid follower.spin. Merry Christmas and Happy New Year to every one!
http://www.youtube.com/watch?v=FlKa65Ui1Xc

Con    ''Stingray ping PID follower by Dave Ratcliff "ratronic" 12/24/10  (with forward facing left and right ping sensors)
  _clkmode = xtal1 + pll16x         ''setup propeller clock @80mhz (5mhz crystal * 16pll)
  _xinfreq        = 5_000_000       ''
 
  lps = 13         ''left ping servo i/o 
  rps = 12         ''right ping servo i/o         
  lpg = 15         ''left ping i/o           
  rpg = 14         ''right ping i/o          
  rsp = 100        ''robot wheel speed 
  pwm = 24         ''pwm base i/o (for stingray's wheel motors)
 
Obj
 
  pd[2] : "pid"           ''pid left and right objects
  pg    : "ping"          ''ping sensor object
  pw    : "pwmx8"         ''pulse width modulation object
  sr    : "servo32v5"     ''servos object
 
Var
 
Pub main | l, r, lp, rp
''initialize
 
  pw.start(pwm, %00001111, 23_000)            ''start pwm object on base i/o for stingray wheel motors drive pins @ 23khz 
  sr.set(lps, 1000)                           ''set left ping servo looking forward
  sr.set(rps, 1500)                           ''set right ping servo looking forward
  sr.start                                    ''start servo object
            ''kp,    ki,   kd,  setpoint, offset, max area, min area
  pd[0].init(-40.0, -3.0, -1.0, 10.0,     0.0,    0.0,      0.0)    ''initialize pid object for left side following @ 10 inches
  pd[1].init(-40.0, -3.0, -1.0, 10.0,     0.0,    0.0,      0.0)    ''initialize pid object for right side following @ 10 inches
 
''main loop
 
  repeat                                      ''keep repeating instructions below
 
    l := pg.inches(lpg)                       ''get left ping sensor distance in inches
    waitcnt(clkfreq/100 + cnt)                ''delay 10ms before next ping distance check (echo reduction)
    r := pg.inches(rpg)                       ''get right ping sensor distance in inches
    lp := pd[0].calculate(l)                  ''get pid motor calculation for left side
    rp := pd[1].calculate(r)                  ''get pid motor calculation for right side
    lp := -rsp #> lp <# rsp                   ''limit left pid motor calculation to rsp constant
    rp := -rsp #> rp <# rsp                   ''limit right pid motor calculation to rsp constant
    move_robot(lp, rp)                        ''set stingray's left and right wheel speed from pid motor calculations
 
 
Pub move_robot(left_wheel, right_wheel)  ''set the speed of stingray's left and right wheels, -255 max.reverse to 255 max.forward, 0=stop               
  left_wheel := -255 #> left_wheel <# 255     ''limit left_wheel motor speed from -255 to 255                                                                                                                                       
  right_wheel := -255 #> right_wheel <# 255   ''limit right_wheel motor speed from -255 to 255
  ifnot left_wheel & $80_00_00_00             ''check sign bit for left_wheel motor speed amount 
    pw.duty(pwm + 1, left_wheel & $ff)        ''set pwm output for left_wheel 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_wheel) & $ff)      ''set pwm output for left_wheel base pin limited to 255
 
  ifnot right_wheel & $80_00_00_00            ''check sign bit for right_wheel motor speed amount 
    pw.duty(pwm + 2, right_wheel & $ff)       ''set pwm output for right_wheel base pin + 2 limited to 255
    pw.duty(pwm + 3, 0)                       ''set pwm output for right_wheel base pin + 3 to 0
  else                                        
    pw.duty(pwm + 2, 0)                       ''set pwm output for right_wheel base pin + 2 to 0
    pw.duty(pwm + 3, (0 - right_wheel) & $ff) ''set pwm output for right_wheel base pin + 3 limited to 255

Roy Eltham

Awesome work! Thanks for sharing!

erco

Sa-Weet! Very nice robot response!

ratronic

Thanks guys. I wanted to mention that you don't have to have servo mounted pings to make this work. Just two forward facing pings mounted to the front of your Stingray spaced somewhat apart. @erco the response time can be stepped up (or down) more than it is set in the posted program. Also using Phil's pwm program for the stingray motors drive full speed is 255 and I have it limited to 100. I set it that way because I'm not too quick on my feet so I slowed down the max speed so I wouldn't have to chase it down in case it got away from me!

Tony B.

ratronic,

Awesome!!! Great work with the Stingray. I'm so glad my ping idea has worked so successfully for you. I hoped it would inspire others and get the ball rolling on developing good software for the Stingray. I had to lay off from working on robotics this summer and fall, but now I am able to get back to work. I will try running your code this week. Could you post more pictures and info on your robot? The description and video have me eager to see more and I could use a little inspiration.

Thanks for sharing your work!
Tony

ratronic

Thanks Tony. I am constantly changing my Stingray configuration around but I am starting to get it more set to how I want it. The network camara(cheap dlink one) transmits to my router so that I can use any computer that's connected to my network using it's web browser to see what my robots looking at (if the arm is not up and in it's way). The ir detector I use with my cable box remote to control the Stingray. The rf transceiver I have used to receive joystick info transmitted from the computer using a usb joystick and a short RobotBasic program to get the joystick info and send rs232 to a transmitter to move the Stingray around and operate the arm/gripper. The motor encoders I just installed and haven't played with to much yet but I have attached a picture of how there setup and the software I been using to experiment with them(theres a thread somewhere about them). The dual pings on servos was a brilliant idea on your part as it made this following demo possible by having both pings face directly forward and making it easy for obstacle detection. The arm/gripper(ax12 digital servos) I just revised to a single servo gripper/2 arm servos. And the 7.2 volt 5000mah LiPo battery makes it possible to operate all this stuff! I don't really have a schematic drawn up for the way it's configured now but if there is some part of this your interested in I can make one up.

doggiedoc

Very cool D Rat!

Martin_H

I missed this thread the first time around. That's a neat PID demo. Also, does that Stingray have a robot arm on top of it?

ratronic

Thanks doggiedoc. @Martin_H Yes that is an arm with a gripper attached to the front. I describe a little how I operate it in post #6 earlier in this thread.

th3jester

It's nice to see a well made PID Object. Control systems is the key.

Happytriger2000

ratronic wrote: »

This is a Stingray PID follower demo using the dual pings setup (thanks Tony B.!) for left and right distance measuring. This demo shows the Stingray following an empty Parallax shipping box @ 10 inches.
For anybody that setup the pings on their Stingray like Tony B. showed in this thread- http://forums.parallax.com/showthread.php?123541-New-Take-on-Pings-for-the-Stingray give this a try. (setting up the proper i/o pins for the pings and their servos in the Con section for your robot)

So far I have added this ping setup, set of motor encoders, ir receiver, rf transceiver, network camara, an arm/gripper, and a 7.2volt 5000mah LiPo battery to my Stingray. All you need though to try this demo
is the Stingray and the ping setup. Download the stingray pid follower.zip file, unzip and click on stingray pid follower.spin. Merry Christmas and Happy New Year to every one!
http://www.youtube.com/watch?v=FlKa65Ui1Xc

Con    ''Stingray ping PID follower by Dave Ratcliff "ratronic" 12/24/10  (with forward facing left and right ping sensors)
  _clkmode = xtal1 + pll16x         ''setup propeller clock @80mhz (5mhz crystal * 16pll)
  _xinfreq        = 5_000_000       ''
 
  lps = 13         ''left ping servo i/o 
  rps = 12         ''right ping servo i/o         
  lpg = 15         ''left ping i/o           
  rpg = 14         ''right ping i/o          
  rsp = 100        ''robot wheel speed 
  pwm = 24         ''pwm base i/o (for stingray's wheel motors)
 
Obj
 
  pd[2] : "pid"           ''pid left and right objects
  pg    : "ping"          ''ping sensor object
  pw    : "pwmx8"         ''pulse width modulation object
  sr    : "servo32v5"     ''servos object
 
Var
 
Pub main | l, r, lp, rp
''initialize
 
  pw.start(pwm, %00001111, 23_000)            ''start pwm object on base i/o for stingray wheel motors drive pins @ 23khz 
  sr.set(lps, 1000)                           ''set left ping servo looking forward
  sr.set(rps, 1500)                           ''set right ping servo looking forward
  sr.start                                    ''start servo object
            ''kp,    ki,   kd,  setpoint, offset, max area, min area
  pd[0].init(-40.0, -3.0, -1.0, 10.0,     0.0,    0.0,      0.0)    ''initialize pid object for left side following @ 10 inches
  pd[1].init(-40.0, -3.0, -1.0, 10.0,     0.0,    0.0,      0.0)    ''initialize pid object for right side following @ 10 inches
 
''main loop
 
  repeat                                      ''keep repeating instructions below
 
    l := pg.inches(lpg)                       ''get left ping sensor distance in inches
    waitcnt(clkfreq/100 + cnt)                ''delay 10ms before next ping distance check (echo reduction)
    r := pg.inches(rpg)                       ''get right ping sensor distance in inches
    lp := pd[0].calculate(l)                  ''get pid motor calculation for left side
    rp := pd[1].calculate(r)                  ''get pid motor calculation for right side
    lp := -rsp #> lp <# rsp                   ''limit left pid motor calculation to rsp constant
    rp := -rsp #> rp <# rsp                   ''limit right pid motor calculation to rsp constant
    move_robot(lp, rp)                        ''set stingray's left and right wheel speed from pid motor calculations
 
 
Pub move_robot(left_wheel, right_wheel)  ''set the speed of stingray's left and right wheels, -255 max.reverse to 255 max.forward, 0=stop               
  left_wheel := -255 #> left_wheel <# 255     ''limit left_wheel motor speed from -255 to 255                                                                                                                                       
  right_wheel := -255 #> right_wheel <# 255   ''limit right_wheel motor speed from -255 to 255
  ifnot left_wheel & $80_00_00_00             ''check sign bit for left_wheel motor speed amount 
    pw.duty(pwm + 1, left_wheel & $ff)        ''set pwm output for left_wheel 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_wheel) & $ff)      ''set pwm output for left_wheel base pin limited to 255
 
  ifnot right_wheel & $80_00_00_00            ''check sign bit for right_wheel motor speed amount 
    pw.duty(pwm + 2, right_wheel & $ff)       ''set pwm output for right_wheel base pin + 2 limited to 255
    pw.duty(pwm + 3, 0)                       ''set pwm output for right_wheel base pin + 3 to 0
  else                                        
    pw.duty(pwm + 2, 0)                       ''set pwm output for right_wheel base pin + 2 to 0
    pw.duty(pwm + 3, (0 - right_wheel) & $ff) ''set pwm output for right_wheel base pin + 3 limited to 255

Hi,
Is there a bs2 version?

thanks

ratronic

No Happytrigger2000 there is not.