Servo control on BSIIsx?
Vern Graner
Posts: 337
Quick question on the BSIIsx:
I am trying to position servos and I'm using the information and sample code shown in the Basic Stamp Manual (v1.9 pg 86):
SERVO:
PULSOUT 0,150
PAUSE 20
GOTO SERVO
UNfortunately, this does not place the servo at "center' but rather "bangs" the servo against the stop. I have tried this with different servos and they react the same. Do I need to use different values for the BSIsx (like I do for the serial baud rate)?
Thanx
Vern
Post Edited (Vern) : 10/20/2004 9:50:47 PM GMT
I am trying to position servos and I'm using the information and sample code shown in the Basic Stamp Manual (v1.9 pg 86):
SERVO:
PULSOUT 0,150
PAUSE 20
GOTO SERVO
UNfortunately, this does not place the servo at "center' but rather "bangs" the servo against the stop. I have tried this with different servos and they react the same. Do I need to use different values for the BSIsx (like I do for the serial baud rate)?
Thanx
Vern
Post Edited (Vern) : 10/20/2004 9:50:47 PM GMT
Comments
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
1us = 1000/.8 = 1250 "bottom" or "0-degrees"
2us = 2000/.8 = 2500 "top" or "180 degrees"
I then wrote this snippet of code to test the formula by seeking the servo through it's entire range:
' {$STAMP BS2sx} cntI VAR Word servo: FOR cntI=1250 TO 2500 PULSOUT 0, cntI PAUSE 10 NEXT FOR cntI=2500 TO 1250 PULSOUT 0, cntI PAUSE 10 NEXT GOTO servoUnfortunately, it only seeks the servo through about 60 degrees. Also, it is rather slow moving. I tried speeding it up by using "step 2" or "step 5" on the FOR statement, that seemed to work.. Don't know if thats the right approach.
Thats not the main problem though. I still don't understand why it is only seeking through such a small range when it would appear I am setting values that should generate a continuous sweep 180 degrees from 1us to 2us..?
Also, if I were to include multiple servos in this code, for example:
Would I have to recalculate the .8 value or change the pause value? I'm also a little unclear on the "pause" time and how I should determine it's value...? Arg. I wish I could just budget for a Serial Servo Controller, but I have to make this on a shoestring budget and If I can get sucessful control of the servos directly, it would cut the project cost in half...
Vern
DO FOR idx = 1 TO 50 PULSOUT 0, 1250 PAUSE 20 NEXT FOR idx = 1 TO 50 PULSOUT 0, 1875 PAUSE 20 NEXT FOR idx = 1 TO 50 PULSOUT 0, 2500 PAUSE 20 NEXT LOOP▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
1. You *must* leave at least 20 mSec between pulses to the Servo. It can't react faster than that.
2. You *should* repeat a pulse to the servo, and give it some time to move.
I'm guessing that since you did all this in a fast loop, that the servo never had time to move all the way to one side, before your code started commanding it to move to the other side.
Jon's solution has two pieces to it -- he repeats each command, and he uses the 20 mSec pause between commands.
By repeating each position command 50 times, with a 20 mSec pause between each PULSOUT, he gives the servo time to move. (A good lab would be to command the servo all the way one way, time its movement, then command it all the other way, and time its movement. This will tell you the fastest rate you can get out of the servo, and how many times you should repeat the command)
However, the speed of seek is not the *primary* problem, in fact, if anything I need the seek to be slower or adjustable as the end result is servos controlling a "puppet" so the fast movements do not appear "natural". What I am looking to make is a series of "positions" that the servos could seek to that I can step through over and over.
Where I am confused is in the timing of the pause statements and the reason my BSIIsx seems to not be able to seek the servo through the entire 180 degree range. Allan theorized that the PULSEOUT command was not being given enough time to seek the servo to it's end position. So, using Jon's example of "seek to zero, then 1/2 way then full rotate: above, I found the servo still only seeks through about a 60 degree arc. In fact, to get the full 180 degree arc, I had to use this code:
' {$STAMP BS2sx} idx VAR Word DO FOR idx = 1 TO 100 PULSOUT 0, 320 PAUSE 20 NEXT FOR idx = 1 TO 100 PULSOUT 0, 1550 PAUSE 20 NEXT FOR idx = 1 TO 100 PULSOUT 0, 2750 PAUSE 20 NEXT LOOPUsing the above I get a full 180 degrees, in three distinct steps. I tried this with two different servos, a Futaba S3004 and Futaba FP-S148, they both required these values to do a full 180 degree seek. Is my BSIIsx "broken" in it's timing?
Also, is there a way to position and hold more than one servo, like this:
FOR idx = 1 TO 100 PULSOUT 0, 2750 PULSOUT 1, 2750 PULSOUT 2, 2750 PULSOUT 3, 2750 PAUSE 20 NEXTShould the "PAUSE" value be altered based on the number of PULSOUT commands? And if this won't work, what should I use for values or is there a "better" way to code this? TIA!
Vern
Post Edited (Vern) : 10/21/2004 5:15:50 PM GMT
You can leave the PAUSE value at 20 -- any value from 15 to 35 is going to be okay; 20 is the standard.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax
Dallas Office
GOF:
INDEX = (DIRA & %0011)
BRANCH, INDEX [noparse][[/noparse]F,F,GOSB,GOPT,ect]
F:
PULSOUT 15, 1500
GOTO GOF
Yes, your last example will work fine. This is why people use Servo's with the BS2sx. The Servo refresh rate is loose enough that anything from 20 mSec to 50 mSec will work -- so you can 'refresh' 4 or 8 servos, then do a single 'Pause 20' before refreshing them again. The servo's will seem to move at the same time (*maybe* theres a few 100 uSecs delay between the actual movements).
I havn't used the BS2sx myself yet. Once you've found the right numbers, however, I think repeatability between different BS2sx should be excellent. It's easy to move servo's slower by stepping to positions close together -- it may still need a few repeats of each command to give the mechanical components time to move.
' {$STAMP BS2sx} ' {$PBASIC 2.5} '------------------------------------------------------ ' Random movement generator ' By Vern Graner 10-22-2004 ' R-1.15a '------------------------------------------------------ 'Declare Variables '------------------------------------------------------ SERVO VAR Nib 'Store the servo number SPEED VAR Byte 'Seek speed for the servos RESULT VAR Word 'Store the random number result CUR VAR Word (4) 'Array to hold the current servo position DEST VAR Word (4) 'Array to hold the destination servo position 'Set the seek speed '------------------------------------------------------ SPEED=15 ' Use RCTIME POT position to generate a random seed '------------------------------------------------------ HIGH 7 PAUSE 1 RCTIME 7, 1, RESULT 'Main Loop that "seeks" servos to destination '------------------------------------------------------ DO SERVO=SERVO+1 'cycle through the servos IF SERVO=4 THEN SERVO = 0 ENDIF IF CUR(SERVO)=DEST(SERVO) THEN 'If the servo has reached GOSUB FetchNewDest 'its destination then fetch ENDIF 'a new random destination IF CUR(SERVO)<DEST(SERVO) THEN 'If the current servo position is LESS CUR(SERVO)=CUR(SERVO)+SPEED 'then incremment by "SPEED" IF CUR(SERVO)>DEST(SERVO) THEN 'Check to be sure the SPEED increment CUR(SERVO)=DEST(SERVO) 'does NOT "overshoot" the destination ENDIF 'value ENDIF IF CUR(SERVO)>DEST(SERVO) THEN 'Same as above, but decrement CUR(SERVO)=CUR(SERVO)-SPEED IF CUR(SERVO)<DEST(SERVO) THEN CUR(SERVO)=DEST(SERVO) ENDIF ENDIF 'Uncomment to observe the values 'If you don't have servos '------------------------------------------------- 'DEBUG CLS 'DEBUG "Dests: 0=", DEC DEST(0)," 1=", DEC DEST(1)," 2=", DEC DEST(2)," 3=",DEC DEST(3),CR 'DEBUG "Curs : 0=", DEC CUR(0)," 1=", DEC CUR(1)," 2=", DEC CUR(2)," 3=",DEC CUR(3),CR PULSOUT SERVO,CUR(SERVO) 'send a pulse to the servo PAUSE 10 'wait for pulse to be "digested" by servo LOOP FetchNewDest: RANDOM RESULT 'Fetch "random" value and place in "RESULT" RESULT=RESULT // 2700 'Limit the result to 0-2700 RESULT=RESULT MIN 220 'Enforce the servo lower limit RESULT=RESULT MAX 2700 'Enforce the servo upper limit DEST(SERVO)=RESULT 'place the "clean" new value as new destination RETURNAgain, any input would be appreciated...
http://www.spiderspreyground.com/howto/dk-toepincher/
The servos control head pan/tilt, eye motion (left/right) and the position of one arm (the other one is static). I sure would love to get a "smoother" motion from this. I have a number of other people that are intersted in replicating this design so getting it down to a stamp, some code and a handful of servos is the optimal goal... Any help/comment would be greatly appreciated.
Vern
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Post Edited (Vern) : 9/21/2006 4:13:41 PM GMT
Your servo comparator may be missing some of your pulses, which might lead to jerky action. Never done this myself, so I'm blue-sky theorizing for that.
OH! I just saw an optimization. You are pausing after EVERY servo update 10 mSec. You only need to have ONE 20 mSec pause after updating ALL FOUR servo's.
IF Servo=4 THEN ' Existing
Servo = 0 ' Existing
PAUSE 20 ' New
ENDIF
and remove the 'PAUSE 10' lower in the code. This way, every time you have updated all 4 servo's, your code will wait 20 mSec for your servo's to move, before updating them to the new positions.
Thanks for the code optimization! That did really help (four pulses, one pause). I guess I still need to get a "feel" for how much time it takes to execute the code. I had imagined the execution of the code would take some time and that I would have to shorten the pause in response to those delays... Guess the BSIIsx is faster than I thought! [noparse]:)[/noparse]
The good news is that I can sucessfully control four servos, running through a series of "Random" movements in a pretty smooth manner to make a mostly realistic looking puppet!
' {$STAMP BS2sx} ' {$PBASIC 2.5} '------------------------------------------------------ ' Servo Random Movement Generator ' By Vern Graner 10-26-2004 ' Any questions to vern@graner.com ' R-10.25b '------------------------------------------------------ ' -Added code to alter the stepping "speed" using an ' array with a seperate "step" rate for each servo ' -Added code to accel/decel servo seek speed ' -Added code to place motion limits on each servo ' -Added "controlled" start/stop '------------------------------------------------------ ' Hardware setup: ' Servos connected to pins 0-3 ' RC time POT ON PIN 7 (used BSAC values) 'Declare Variables '------------------------------------------------------ SERVO VAR Nib 'Store the servo number I VAR Nib 'Standard "for" counter RESULT VAR Word 'Store the random number result RAMP VAR Bit (4) 'Store speed ramp direction SPEED VAR Byte (4) 'Seek speed for the servos CUR VAR Word (4) 'Array to hold the current servo position DEST VAR Word (4) 'Array to hold the destination servo position 'initialize the seek speed for each servo '------------------------------------------------------ FOR I = 0 TO 3 SPEED(0)=1 NEXT I 'Set the "start position" for each servo '------------------------------------------------------ GOSUB LIMITS 'Position servos within their "safe" area FOR I = 0 TO 3 '(usually center). by "fetching" the value CUR(I) = DEST(I) ' from the "limits" subroutine and pre-stuffing NEXT ' them in to the "CURrent" value for each servo ' Use RCTIME POT position to generate a random seed '------------------------------------------------------ HIGH 7 PAUSE 1 RCTIME 7, 1, RESULT 'Main Loop that "seeks" servos to destination '------------------------------------------------------ DO SERVO=SERVO+1 'cycle through the four servos IF SERVO=4 THEN PAUSE 20 'AFTER sending four pulses to the four servos SERVO = 0 'wait for 20us to allow the pulse to ENDIF 'be "digested" by servo IF RAMP(SERVO) = 1 THEN 'Based on "ramp" direction, either increase SPEED(SERVO)=SPEED(SERVO)+1 'or decrease the servo seek speed by 1 ELSE SPEED(SERVO)=SPEED(SERVO)-1 ENDIF SPEED(SERVO)=SPEED(SERVO) MAX 50 'Enforce a maximum and minimum seek speed SPEED(SERVO)=SPEED(SERVO) MIN 10 IF CUR(SERVO)=DEST(SERVO) THEN 'If the servo has reached GOSUB FetchNewDest 'its destination then fetch ENDIF 'a new destination IF CUR(SERVO)<DEST(SERVO) THEN 'If the current servo position is LESS CUR(SERVO)=CUR(SERVO)+SPEED(SERVO) 'then incremment by "SPEED" IF CUR(SERVO)>DEST(SERVO) THEN 'Check to be sure the SPEED increment CUR(SERVO)=DEST(SERVO) 'does NOT "overshoot" the destination ENDIF 'value ENDIF IF CUR(SERVO)>DEST(SERVO) THEN 'Same as above, but decrement CUR(SERVO)=CUR(SERVO)-SPEED(SERVO) IF CUR(SERVO)<DEST(SERVO) THEN CUR(SERVO)=DEST(SERVO) ENDIF ENDIF 'Uncomment to observe the values 'If you don't have servos '------------------------------------------------- 'DEBUG CLS 'DEBUG "Ramp : 0=", DEC RAMP(0)," 1=", DEC RAMP(1)," 2=", DEC RAMP(2)," 3=",DEC RAMP(3),CR 'DEBUG "Speed: 0=", DEC SPEED(0)," 1=", DEC SPEED(1)," 2=", DEC SPEED(2)," 3=",DEC SPEED(3),CR 'DEBUG "Dests: 0=", DEC DEST(0)," 1=", DEC DEST(1)," 2=", DEC DEST(2)," 3=",DEC DEST(3),CR 'DEBUG "Curs : 0=", DEC CUR(0)," 1=", DEC CUR(1)," 2=", DEC CUR(2)," 3=",DEC CUR(3),CR PULSOUT SERVO,CUR(SERVO) 'send a pulse to the servo LOOP 'SUBROUTINE: Generate new destination value '-------------------------------------------------- 'Note: rather than using "RANDOM" you could create 'an array of predefined servo positions and step through 'them to create a repeatable series of motions FetchNewDest: HIGH 7 'Use RC time to gather a new "Seed" for RANDOM PAUSE 1 'move the POT around on occasion to change RCTIME 7, 1, RESULT 'the up the show. Due to "jitter" the RC time value 'can introduce it's own "variances" to the seed 'value resulting in more "randomness" IF RESULT = 0 THEN RETURN 'Controlled stop/start. Added this so I could 'move the POT to "zero" and the motions all finish 'their last seek to destination and then halt. makes 'for a more "natural" halt than the sudden "freeze" 'of all axis of motion. Move POT away from zero and 'the motion restarts. If you don't have a POT for 'RC time, this could be replaced with a button command. RANDOM RESULT 'Fetch "random" value and place in "RESULT" RESULT=RESULT // 2700 'Limit the result to 0-2700 DEST(SERVO)=RESULT 'place the "clean" new value as new destination GOSUB LIMITS IF RAMP(SERVO) = 0 THEN ' Change the speed ramp direction RAMP(SERVO) = 1 ' 0 causes the speed to increase ELSE ' 1 causes speed to decrease RAMP(SERVO)=0 ENDIF RETURN 'SUBROUTINE: Enforce the servo limits '-------------------------------------------------- Limits: DEST(0)=DEST(0) MAX 2700 'Servo 0 MAX (eye pan) DEST(0)=DEST(0) MIN 320 'Servo 0 MIN DEST(1)=DEST(1) MAX 520 'Servo 1 MAX (head tilt) DEST(1)=DEST(1) MIN 320 'Servo 1 MIN DEST(2)=DEST(2) MAX 1000 'Servo 2 MAX (head pan) DEST(2)=DEST(2) MIN 700 'Servo 2 MIN DEST(3)=DEST(3) MAX 2700 'Servo 3 MAX (arm reach) DEST(3)=DEST(3) MIN 320 'Servo 3 MIN RETURNThanks again for everyone's help, and if anyone has any other comments, or questions, please let me know! This has been a very enlightening experience and shows me the stamp is capable of operating multiple servos w/o having to resort to a PWM pal or Serial Servo Controller! [noparse]:)[/noparse]
Vern
Post Edited (Vern) : 10/26/2004 3:11:08 PM GMT
I really like how you've made use of the arrays, and the subroutines to produce relatively simple code that does quite complicated things. Very nicely structured code *and* data.
Going through your code I would suggest this based on my game programming and refresh rates of video output.
When programming for smooth output of 3D or 2D video, one packs as much processing of the numbers between the vertical scan
as the screen gets painted from the top left corner all the way to the bottom right corner it has to move the gun back to the top right corner.
During that time your program crunches numbers and puts the new screen update in a seperate buffer of memory.
Your code can do the same thing. First calculate the amount of time your crunching of numbers takes to do all 4 servo calculations
if its under 20 ms then subtract that number from 20 to get your pause time.
Send all 4 servos the new positions at one time
crunch the next set of numbers
repeat
I dont know because I just started working with pbasic, if there is a timer function you can use to get an exact execution time for each time the code runs thru so you can calculate the pause time for each set of servo commands sent but if you do all 4 calculations every time it should be pretty close.
If your code takes longer than 20 ms you could do 2 servos at a time but remember the code will take different times to execute.
I am thinking the jerky part is the physics of the model you are moving as the force of each servo enguages one after the other creating a worse condition depending on the actual direction of the movement. for example one arm moves forward servo1 and them servo 2 moves the other arm backwards, the momentum of the first arm going forward effects the balance of the model and when the second servo enguages your model may already have some energy propelling it backwards if the forward movement of the first arm caused the model to spring or lean foreward like a pendulum.
Thanks for the feedback. That is exactly what I was looking for. To be honest, this code "grew" ( kinda like "mold"!
Anywya, I appreciate the feedback as I do hope tooptimize the code at some point so that I can use it to "interpret" strings of coordinates that are "recorded". Ideal case would be a joystick type input that I could run through and record motions into an array, then run the array back... This would make it simple to animate a servo-controlled prop.. That'll have to wait since I'm out of resources for the moment... my haunted house is this friday so I'm out of time, and the code I wrote eats about all the RAM in the BSIIsx (those "WORD" sized arrays to hold the servo positions eat the space!) Maybe I can figure out how to use less ram later.
Vern
You may find the following link helpful in determining the exectution time of various Stamp instructions:
http://www.emesystems.com/BS2speed.htm
Thanks go to Dr. Tracy Allen, for all his hard work in determining these instruction speeds for the BS-2, and BS-2sx platforms.
Regards,
Bruce Bates
A "Pause 20" pauses for 20 mSec (milli-seconds) not 20 uSec (micro-seconds). 20 mSec is a *much* longer time in BS2 land.
A BS2sx does one 'primitive' PBasic instruction in around 60 uSec ("HIGH 1"). so it can do about 18 instructions per milli-second, or 357 instructions in 20 mSec. The time for instructions like "PAUSE xx", SEROUT, etc. are driven more by the data than the time to actually execute the instruction.
Servo's need a 20 mSec to 50 mSec delay before repeating the pulse, so the "Pause 20" is correct.
Then average the time (using a stopwatch to measure the time it takes to execute the loop)
As each pulse is received (every 20milliseconds or so) the servo will move a little closer to it's position.
So the number of pulses required to actually reach the position depends on where it started.
So you can see how confused the servo gets if you are sending different pulse widths all the time.
You should send the pulse width for some time to reach the desired position.
Bean.