' -----[ Title ]----------------------------------------------------------------------- ' For Demonstration - Bot1CompleteMovementsWithComunicationAndMusicTopBoard.bs2 ' Date: 18 May 08 ' ' ' -----[ Notes ]----------------------------------------------------------------------- ' ' *** Gripper *** ' Have bot 1 Open and close its gripper along with moving the gripper up and down. ' Note: the Control horn has four "blades" ' We have the Control horn positioned innitially with the blade - that has the the gripper rod ' going through - over the Parallax standard servo text. ' ' Revision: the initial condition is the same as before but the blade is turned about 15 degrees CW. ' ' *** Gripper up/down motion *** ' Changed the counter value from 50 to 40 due to the fact that the gripper assembly ' interfeared with the gripper support. ' ' Changed the counter value form 40 to 10 due to the fact that the gripper assembly ' hits the top board otherwise. ' ' Used rechargeable 1.2V batteries. ' ' ' -----[ Program ]--------------------------------------------------------------------- '{$STAMP BS2} 'Stamp directive. '{$PBASIC 2.5} 'PBASIC directive. ' -----[ Variables/Constants/Pins ]---------------------------------------------------- ' Stuff for music to work Green1Led PIN 1 Green2Led PIN 3 Green3Led PIN 5 YellowLed PIN 6 C6 CON 1047 ' Piano notes C6_sharp CON 1109 D6_flat CON 1109 D6 CON 1175 D6_sharp CON 1245 E6_flat CON 1245 E6 CON 1319 F6 CON 1397 F6_sharp CON 1480 G6_flat CON 1480 G6 CON 1568 G6_sharp CON 1661 A6_flat CON 1661 A6 CON 1760 A6_sharp CON 1865 Be6_flat CON 1865 Be6 CON 1976 C7 CON 2093 C7_sharp CON 2218 D7_flat CON 2218 D7 CON 2349 D7_sharp CON 2489 E7_flat CON 2489 E7 CON 2637 F7 CON 2793 F7_sharp CON 2960 G7_flat CON 2960 G7 CON 3136 G7_sharp CON 3322 A7_flat CON 3322 A7 CON 3520 A7_sharp CON 3729 Be7_flat CON 3729 Be7 CON 3951 C8 CON 4186 Rest CON 0 ' end of piano notes ' 60s/min * 1000ms/s * 1/(q_n/min) * 4q_n/w_n = ms/w_n. whole CON 1000 * 60 / 150 * 4 ' Timing half CON whole/2 quarter CON whole/4 eighth CON whole/8 sixteenth CON whole/16 thirty_second CON whole/32 whole_dot CON whole + half half_dot CON half + quarter quarter_dot CON quarter + eighth eighth_dot CON eighth + sixteenth ' end of timing 'variables for IR which picks up Lego FreqSelect VAR Nib IrFrequency VAR Word IrDetect VAR Bit counter1 VAR Word counter VAR Word PulseCount VAR Word pulseWidth VAR Word ' Period of PULSOUT command for gripper StepSize CON 10 ' Step size for gripper using newer servo. CloseGrip CON 900 OpenGrip CON 600 FreqDetectable CON 3000 GripperMover PIN 14 ' Rotates gripper Gripper PIN 15 ' Gripper servo Piezospeaker PIN 4 ' Speaker TopBump PIN 10 BottomBump PIN 9 InPin PIN 12 ' Used as IN12 and IN11 OutPin PIN 11 TreadServo PIN 13 IrDetectRight PIN 0 'Right emitter/detector pair IrLedRight PIN 2 ' -----[ Initialization ]-------------------------------------------------------------- FREQOUT Piezospeaker, 2000, FreqDetectable 'Signal program start/reset. ' -----[ Main Routine ]---------------------------------------------------------------- PAUSE 4000 GOSUB first ' Low intro. GOSUB second ' First series of high notes GOSUB third ' Second series of high notes GOSUB fourth ' Crecindo GOSUB Movements first: FREQOUT Piezospeaker, eighth, Rest HIGH Green1Led FREQOUT Piezospeaker, eighth, G6 LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6 LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, G6 LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, half, D6_sharp LOW YellowLed 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH YellowLed FREQOUT Piezospeaker, half, Rest 'LOW YellowLed RETURN second: FREQOUT Piezospeaker, eighth, Rest HIGH Green1Led FREQOUT Piezospeaker, eighth, G6 LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6 LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, G6 LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, eighth, D6_sharp LOW YellowLed HIGH Green1Led FREQOUT Piezospeaker, eighth, G6_sharp LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6_sharp LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, G6_sharp LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, eighth, G6 LOW YellowLed HIGH Green1Led FREQOUT Piezospeaker, eighth, D7_sharp LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, D7_sharp LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, D7_sharp LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, half, C7 LOW YellowLed RETURN third: FREQOUT Piezospeaker, eighth, Rest 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH YellowLed FREQOUT Piezospeaker, eighth, Rest 'LOW YellowLed 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH YellowLed FREQOUT Piezospeaker, eighth, Rest 'LOW YellowLed 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH YellowLed FREQOUT Piezospeaker, half, Rest 'LOW YellowLed RETURN fourth: FREQOUT Piezospeaker, eighth, Rest HIGH Green1Led FREQOUT Piezospeaker, eighth, G6 LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6 LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, F6 LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, eighth, D6_sharp LOW YellowLed 'HIGH YellowLed FREQOUT Piezospeaker, eighth, Rest 'LOW YellowLed 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led HIGH Green1Led FREQOUT Piezospeaker, eighth, G6 LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6 LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, F6 LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, eighth, D6_sharp LOW YellowLed 'HIGH YellowLed FREQOUT Piezospeaker, eighth, Rest 'LOW YellowLed 'HIGH Green3Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green3Led 'HIGH Green2Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green2Led 'HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest 'LOW Green1Led HIGH Green1Led FREQOUT Piezospeaker, eighth, G6 LOW Green1Led HIGH Green2Led FREQOUT Piezospeaker, eighth, G6 LOW Green2Led HIGH Green3Led FREQOUT Piezospeaker, eighth, F6 LOW Green3Led HIGH YellowLed FREQOUT Piezospeaker, eighth, D6_sharp LOW YellowLed FREQOUT Piezospeaker, quarter_dot, Rest HIGH YellowLed FREQOUT Piezospeaker, eighth, F6 LOW YellowLed FREQOUT Piezospeaker, quarter_dot, Rest HIGH YellowLed FREQOUT Piezospeaker, quarter, F6_sharp LOW YellowLed RETURN Movements: FOR counter1 = 1 TO 3 Top: DO IF (IN12 = 1) THEN GOSUB IR_Detection HIGH OutPin PAUSE 500 LOW OutPin GOSUB SecondLoop ENDIF LOOP SecondLoop: DO IF (IN12 = 1) THEN GOSUB Part2 ENDIF LOOP Part2: GOSUB MoveGripDown PAUSE 500 GOSUB Up Rest1: PAUSE 500 GOSUB OpenGripper PAUSE 500 GOSUB CloseGripper PAUSE 500 GOSUB OpenGripper PAUSE 500 GOSUB Down Rest2: PAUSE 15000 GOSUB UpHalf PAUSE 2000 GOSUB MoveGripUp HIGH OutPin PAUSE 500 LOW OutPin NEXT ' -----[ Sub Routines ]---------------------------------------------------------------- IR_Detection: DO 'Main loop FOR FreqSelect = 0 TO 1 LOOKUP FreqSelect, [37500,38250], IrFrequency FREQOUT IrLedRight, 1, IrFrequency IrDetect = IN0 IF (IrDetect = 0) THEN PAUSE 3200 GOSUB MoveGripDown GOSUB CloseGripper GOSUB MoveGripUp RETURN ENDIF PAUSE 50 NEXT 'FreqSelect = 0 TO 1 LOOP 'Main loop Up: DO PULSOUT TreadServo, 740 PAUSE 40 GOSUB Check_top_bump LOOP UpHalf: FOR counter = 1 TO 100 PULSOUT TreadServo, 740 PAUSE 10 NEXT RETURN Check_top_bump: IF (IN10 = 1) THEN GOSUB Rest1 ENDIF RETURN Down: DO PULSOUT TreadServo, 760 PAUSE 10 GOSUB Check_bottom_bump LOOP Check_bottom_bump: IF (IN9 = 1) THEN FOR counter = 1 TO 5 PULSOUT TreadServo, 740 PAUSE 10 NEXT GOSUB Rest2 ENDIF RETURN MoveGripDown: FOR counter = 1 TO 10 PULSOUT GripperMover, 755 PAUSE 20 NEXT RETURN CloseGripper: FOR pulseWidth = OpenGrip TO CloseGrip STEP StepSize PULSOUT Gripper, pulseWidth PAUSE 20 NEXT RETURN MoveGripUp: FOR counter = 1 TO 6 PULSOUT GripperMover, 735 PAUSE 20 NEXT RETURN OpenGripper: FOR pulseWidth = CloseGrip TO OpenGrip STEP StepSize PULSOUT Gripper, pulseWidth PAUSE StepSize NEXT RETURN