' {$STAMP BS2} ' {$PBASIC 2.5} '------------------------------------------------------ ' Servo Random Movement Generator ' Using the PSC (Parallax Servo Controller) ' Created By Vern Graner 10-26-2004 ' Updated By Vern Graner 10-14-2005 ' Any questions to vern@graner.com ' R-1.0 '------------------------------------------------------ ' -Added code to place motion limits on each servo '------------------------------------------------------ ' Hardware setup: ' PSC connected to the Pin 0 'Declare Variables '------------------------------------------------------ SERVO VAR Nib 'Store the servo number I VAR Nib 'Standard "for" counter RESULT VAR Word 'Store the random number result SPEED VAR Byte (3) 'Seek speed for the servos CUR VAR Word (3) 'Array to hold the current servo position DEST VAR Word (3) 'Array to hold the destination servo position PSC1 CON 0 'Pin to which the PSC is connected BAUD CON 396 '2400 baud on BSII BUFF VAR Byte (3) 'array to receive the 3 values from the servo position query 'set the seek speed for each servo '------------------------------------------------------ FOR I = 0 TO 2 SPEED(I)=10 NEXT 'Set the "start position" for each servo '------------------------------------------------------ GOSUB LIMITS 'Position servos within their "safe" area FOR I = 0 TO 2 ' 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 ' Set Random Seed Value '------------------------------------------------------ RESULT = 2234 'Seed the RANDOM number generator ' Start each of the servos seeking to position '------------------------------------------------------ FOR SERVO = 0 TO 2 SEROUT PSC1, Baud+$8000,["!SC", servo, SPEED(SERVO), DEST.LOWBYTE(SERVO*2), DEST.HIGHBYTE(SERVO*2), CR] NEXT 'PAUSE 10000 'Main Loop that checks to see if servos have arrived 'then fetches a new desitnation if so. '------------------------------------------------------ DO SERVO=SERVO+1 'cycle through the servos IF SERVO>2 THEN SERVO = 0 ENDIF 'PAUSE 250 'Give the PSC a bit of time to seek SEROUT PSC1, Baud+$8000, ["!SCRSP", SERVO, CR] 'Send a query to the PSC SERIN PSC1, Baud, [STR Buff\3] 'Fetch an answer from the PSC CUR(SERVO) = (buff(1) << 8) | buff(2) ' reassemble the low/hi bytes 'and stuff the current position IF CUR(SERVO)=DEST(SERVO) THEN 'If the servo has reached GOSUB FetchNewDest 'its destination then fetch ENDIF 'a new destination 'Uncomment to observe the values 'If you don't have servos '------------------------------------------------- DEBUG HOME DEBUG "Servo=",DEC SERVO,CR DEBUG "CUR(servo)=",DEC CUR(SERVO)," ",CR DEBUG "Speed: 0=", DEC SPEED(0)," 1=", DEC SPEED(1)," 2=", DEC SPEED(2)," ",CR DEBUG "Dests: 0=", DEC DEST(0)," 1=", DEC DEST(1)," 2=", DEC DEST(2)," ",CR DEBUG "Curs : 0=", DEC CUR(0)," 1=", DEC CUR(1)," 2=", DEC CUR(2)," ",CR 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: RANDOM RESULT 'Fetch "random" value and place in "RESULT" RESULT=RESULT // 1250 'Limit the result to 0-2700 DEST(SERVO)=RESULT 'place the "clean" new value as new destination GOSUB LIMITS DEBUG CR DEBUG "New Destination generated:",CR SEROUT PSC1, Baud+$8000,["!SC", servo, SPEED(SERVO), DEST.LOWBYTE(SERVO*2), DEST.HIGHBYTE(SERVO*2), CR] DEBUG "Servo=",DEC SERVO," send PSC=",DEC dest.LOWBYTE(servo*2), DEC dest.HIGHBYTE(servo*2),CR RETURN 'SUBROUTINE: Enforce the servo limits '-------------------------------------------------- Limits: DEST(0)=DEST(0) MAX 1000 'Servo 0 MAX (eye pan) DEST(0)=DEST(0) MIN 250 'Servo 0 MIN DEST(1)=DEST(1) MAX 1000 'Servo 1 MAX (head tilt) DEST(1)=DEST(1) MIN 450 'Servo 1 MIN DEST(2)=DEST(2) MAX 1000 'Servo 2 MAX (head pan) DEST(2)=DEST(2) MIN 550 'Servo 2 MIN RETURN