' -----[ Title ]----------------------------------------------------------------------- 'Final - Bot2_Movements_with_Ping_comunications_and_song.bs2 ' ' -----[ Notes ]----------------------------------------------------------------------- ' ' Needed more power to lift up the gripper. Used code created in Testing_CD_motor_more_power.bs2. ' ' ' Used 9V battery. ' ' ' -----[ Program ]--------------------------------------------------------------------- '{$STAMP BS2} 'Stamp directive. '{$PBASIC 2.5} 'PBASIC directive. ' -----[ Variables/Constants/Pins ]---------------------------------------------------- ' Suff for song Green1Led PIN 1 Green2Led PIN 11 Green3Led PIN 12 YellowLed PIN 13 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 ' stuff for ping Ping PIN 0 Trigger CON 5 ' (5 2microsecond pullses) trigger pulse = 10 uS Scale CON 512 ' raw x 2.00 = 1 uS RawToIn CON 889 ' 1 / 73.746 (with **) IsHigh CON 1 ' for PULSOUT IsLow CON 0 FreqDetectable CON 3000 rawDist VAR Word ' raw measurement inches VAR Word counter VAR Word Sense VAR Word ' sensor raw reading ' Gripper var and con. pulseWidth VAR Word StepSize CON 20 ' Step size for gripper using newer servo. CloseGrip CON 450 OpenGrip CON 300 countering VAR Byte OutPin PIN 14 InPin PIN 15 Gripper PIN 12 ' Gripper Piezospeaker PIN 4 ' Speaker LineSnsrPwr PIN 0 ' line sensor power LineSnsrIn PIN 1 ' line sensor input ' CD servo and laser servo_up1 PIN 5 servo_up2 PIN 6 servo_up3 PIN 7 servo_up4 PIN 8 servo_down1 PIN 9 servo_down2 PIN 10 laser PIN 2 ' -----[ Initialization ]-------------------------------------------------------------- FREQOUT Piezospeaker, 2000, FreqDetectable 'Signal program start/reset. LOW OutPin ' -----[ 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 Top first: 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 ''''''''''''''''HIGH Green1Led FREQOUT Piezospeaker, eighth, Rest '''''''''''''''''''''''''''''HIGH Green1Led FREQOUT Piezospeaker, eighth, F6 ''''''''''''''''''LOW Green1Led '''''''''''''''''HIGH Green2Led FREQOUT Piezospeaker, eighth, F6 '''''''''''''''''''''LOW Green2Led ''''''''''''''HIGH Green3Led FREQOUT Piezospeaker, eighth, F6 ''''''''''''''''''''LOW Green3Led '''''''''''''''HIGH YellowLed FREQOUT Piezospeaker, half, D6 '''''''''''''''''LOW YellowLed RETURN second: 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 third: 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 '''''''''''''''''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, F7 ''''''''''''''''LOW Green1Led ''''''''''''HIGH Green2Led FREQOUT Piezospeaker, eighth, F7 ''''''''''''''''''''LOW Green2Led '''''''''''HIGH Green3Led FREQOUT Piezospeaker, eighth, F7 '''''''''''''''LOW Green3Led ''''''''''''''HIGH YellowLed FREQOUT Piezospeaker, half, D7 '''''''''''''''''''''''LOW YellowLed RETURN fourth: 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 YellowLed FREQOUT Piezospeaker, eighth, C7 ''''''''''''''LOW YellowLed ''''''''''''''''''HIGH Green3Led FREQOUT Piezospeaker, eighth, C7 '''''''''''''''LOW Green3Led ''''''''''''''''''HIGH Green2Led FREQOUT Piezospeaker, eighth, D7 '''''''''''''LOW Green2Led '''''''''''''''HIGH Green1Led FREQOUT Piezospeaker, eighth, D7_sharp ''''''''''''''LOW Green1Led '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 YellowLed FREQOUT Piezospeaker, eighth, C7 '''''''''''''LOW YellowLed ''''''''''''HIGH Green3Led FREQOUT Piezospeaker, eighth, C7 ''''''''''''LOW Green3Led '''''''''''''HIGH Green2Led FREQOUT Piezospeaker, eighth, D7 '''''''''''''LOW Green2Led '''''''''''HIGH Green1Led FREQOUT Piezospeaker, eighth, D7_sharp ''''''''''LOW Green1Led '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 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 FOR countering = 1 TO 3 Top: DO IF (IN15 = 1) THEN GOSUB Pinging Rest1: PAUSE 25000 GOSUB CloseGripper PAUSE 500 HIGH OutPin PAUSE 500 LOW OutPin GOSUB Top_part_b: ENDIF LOOP Top_part_b: DO IF (IN15 = 1) THEN GOSUB Down Rest2: GOSUB OpenGripper Rest3: GOSUB Up Rest4: HIGH OutPin PAUSE 500 LOW OutPin GOSUB ending ENDIF PAUSE 40 LOOP ending: NEXT Pinging: DEBUG "pinging" DO GOSUB Get_Sonar ' get sensor value inches = rawDist ** RawToIn ' convert to inches DEBUG DEC inches IF (inches < 24) THEN FREQOUT Piezospeaker, 100, inches/2*1000 ENDIF IF (inches < 7) THEN GOSUB Rest1 ENDIF PAUSE 50 LOOP ' -----[ Subroutines ]----------------------------------------------------- ' This subroutine triggers the Ping sonar sensor and measures ' the echo pulse. The raw value from the sensor is converted to ' microseconds based on the Stamp module in use. This value is ' divided by two to remove the return trip -- the result value is ' the distance from the sensor to the target in microseconds. Get_Sonar: Ping = IsLow ' make trigger 0-1-0, connect ping to ground (0) PULSOUT Ping, Trigger ' activate sensorn, connect ping to vdd (1) PULSIN Ping, IsHigh, rawDist ' measure echo pulse, connect ping to ground (0) rawDist = rawDist */ Scale ' convert to uS rawDist = rawDist / 2 ' remove return trip RETURN Down: DO LOW servo_up1 LOW servo_up2 LOW servo_up3 LOW servo_up4 HIGH servo_down1 HIGH servo_down2 PAUSE 200 LOW servo_down1 LOW servo_down2 GOSUB Bottom_bump_sensor LOOP Bottom_bump_sensor: IF (IN3 = 1) THEN PAUSE 600 GOSUB Rest2 ENDIF RETURN Up: DEBUG "UP" LOW servo_down1 LOW servo_down2 HIGH servo_up1 HIGH servo_up2 HIGH servo_up3 HIGH servo_up4 PAUSE 200 LOW servo_up1 LOW servo_up2 LOW servo_up3 LOW servo_up4 GOSUB Read_laser_Sensor Read_laser_Sensor: IF (IN2 = 1) THEN DEBUG "hit" GOSUB Rest4 ENDIF GOSUB Up CloseGripper: FOR pulseWidth = OpenGrip TO CloseGrip STEP StepSize PULSOUT Gripper, pulseWidth PAUSE StepSize NEXT RETURN OpenGripper: FOR pulseWidth = CloseGrip TO OpenGrip STEP StepSize PULSOUT Gripper, pulseWidth PAUSE StepSize NEXT RETURN