' ========================================================================= ' {$PORT COM1} ' ' File....... Random_Servos.BS1 ' Purpose.... ' Author..... Vern Graner (updated by Jon Williams) ' E-mail..... ' Started.... May 2 2005 ' Updated.... Nov 4th 2005 ' ' {$STAMP BS1} ' {$PBASIC 1.0} ' ' ========================================================================= ' -----[ Program Description ]--------------------------------------------- ' random motions of Robot Arms & head. Random power up/down of spider legs ' -----[ Revision History ]------------------------------------------------ test: ' PWM 1, 3, 150 ' PWM 1, 4, 150 ' PWM 1, 5, 150 ' PWM 1, 6, 150 ' HIGH 1 ' PAUSE 50 'LOW 1 'PAUSE 2000 'GOTO test ' -----[ I/O Definitions ]------------------------------------------------- SYMBOL RightClaw = 0 SYMBOL RightElbow = 1 SYMBOL RightShoulder = 2 SYMBOL LeftClaw = 3 SYMBOL LeftElbow = 4 SYMBOL LeftShoulder = 5 SYMBOL HeadPan = 6 SYMBOL ToSSC = 0 ' Mini SSC pin SYMBOL Sync = 255 ' Mini SSC sync byte. SYMBOL SpiderLegs = 1 ' ULN2803 controlled motor ' -----[ Constants ]------------------------------------------------------- 'SYMBOL LastServo = 6 ' -----[ Variables ]------------------------------------------------------- SYMBOL lottery = W0 ' random number SYMBOL pos1 = B2 ' servo postion 1 SYMBOL dest1 = B3 ' destination 1 SYMBOL pos2 = B4 SYMBOL dest2 = B5 SYMBOL pos3 = B6 SYMBOL dest3 = B7 SYMBOL pos4 = B8 SYMBOL dest4 = B9 SYMBOL pos5 = B10 SYMBOL dest5 = B11 SYMBOL cntI = B12 'Counter 1 'SYMBOL cntX = B13 'Counter 2 ' -----[ EEPROM Data ]----------------------------------------------------- ' -----[ Initialization ]-------------------------------------------------- Reset: lottery = 1031 ' -----[ Program Code ]---------------------------------------------------- Main: RANDOM lottery ' stir the pot ' ========= ' Servo 1 ' ========= Check_Dest1: IF pos1 <> dest1 THEN Update_Pos1 ' are we there yet? dest1 = lottery // 101 + 100 ' scale to 100 - 200 SEROUT ToSSC,N2400,(sync,RightElbow,Dest1) ' Right Elbow Update_Pos1: IF pos1 < dest1 THEN Go_CW1 pos1 = pos1 - 2 Go_CW1: pos1 = pos1 + 1 ' ========= ' Servo 2 ' ========= Check_Dest2: IF pos2 <> dest2 THEN Update_Pos2 dest2 = lottery // 101 + 100 SEROUT ToSSC,N2400,(sync,RightShoulder,Dest2) ' Right Shoulder Update_Pos2: IF pos2 < dest2 THEN Go_CW2 pos2 = pos2 - 2 Go_CW2: pos2 = pos2 + 1 ' ========= ' Servo 3 ' ========= Check_Dest3: IF pos3 <> dest3 THEN Update_Pos3 dest3 = lottery // 101 + 40 SEROUT ToSSC,N2400,(sync,LeftElbow,Dest3) ' Left Elbow Update_Pos3: IF pos3 < dest3 THEN Go_CW3 pos3 = pos3 - 2 Go_CW3: pos3 = pos3 + 1 ' ========= ' Servo 4 ' ========= Check_Dest4: IF pos4 <> dest4 THEN Update_Pos4 dest4 = lottery // 21 + 40 SEROUT ToSSC,N2400,(sync,LeftShoulder,Dest4) ' Left Shoulder Update_Pos4: IF pos4 < dest4 THEN Go_CW4 pos4 = pos4 - 2 Go_CW4: pos4 = pos4 + 1 ' ========= ' Servo 5 ' ========= Check_Dest5: IF pos5 <> dest5 THEN Update_Pos5 dest5 = lottery // 101 + 50 HIGH SpiderLegs PAUSE 50 LOW SpiderLegs PAUSE 50 SEROUT ToSSC,N2400,(sync,HeadPan,Dest5) ' Head Pan HIGH SpiderLegs PAUSE 50 LOW SpiderLegs PAUSE 50 Update_Pos5: IF pos5 < dest5 THEN Go_CW5 pos5 = pos5 - 2 Go_CW5: pos5 = pos5 + 1 Update_Servos: 'Mini SSC Code here: 'DEBUG pos1 cntI = cntI + 1 'DEBUG cntI IF cntI < 253 THEN NoReset cntI=0 NoReset: IF cntI > 50 THEN RightClawOpen ' DEBUG "Right Claw Close",CR SEROUT ToSSC,N2400,(sync,RightClaw,10) ' Right claw close GOTO RightClawDone RightClawOpen: ' DEBUG "Right Claw Open",CR SEROUT ToSSC,N2400,(sync,RightClaw,100) ' Right claw Open RightClawDone: IF cntI > 75 THEN LeftClawOpen ' DEBUG "Left Claw Close",CR SEROUT ToSSC,N2400,(sync,LeftClaw,50) ' Right claw close GOTO LeftClawDone LeftClawOpen: ' DEBUG "Left Claw Open",CR - SEROUT ToSSC,N2400,(sync,LeftClaw,150) ' Right claw Open LeftClawDone: 'IF cntI > 120 THEN HeadPanRight ' SEROUT ToSSC,N2400,(sync,HeadPan,40) ' Right claw close ' GOTO HeadPanDone 'HeadPanRight: ' SEROUT ToSSC,N2400,(sync,HeadPan,120) ' Right claw Open 'HeadPanDone: 'IF cntI < 251 THEN SpiderLegsStop ' HIGH SpiderLegs ' PAUSE 50 ' ' GOTO SpiderLegsDone 'SpiderLegsStop: ' LOW SpiderLegs 'SpiderLegsDone: IF cntI < 252 THEN NoPause PAUSE 7000 NoPause: GOTO MAIN 'lets do it again! :)