' -----[ Title ]-------------------------------------------------------------- ' File: BoeBotControlForMsrs.bs2 ' {$STAMP BS2} ' {$PBASIC 2.5} ' -----[ DATA ]--------------------------------------------------------------- ' eb500 interface 'RX CON 0 ' Serial pins 'TX CON 1 'EasyBT interface RX CON 2 ' Serial pins TX CON 0 ResetOnOff DATA 0 ' On/off toggle w/ Reset button RequestConnect DATA $FF, 0, 1, 0, 0 ConnectionGranted DATA $FF, 0, 2, 0, 0 RequestCommand DATA $FF, 0, 3, 0, 0 ' -----[ Variables ]---------------------------------------------------------- tLeft VAR Word ' Servo control pulse durations tRight VAR Word temp VAR Word ' Temp variable ' Buffer array not declared as buffer VAR Word(5) for SERIN functionality. ' It can still be accessed as buffer(0), buffer(1), etc. However, ' buffer0, buffer1, etc. should be used in SERIN commands with variations ' of WAIT. buffer1 VAR Byte ' Sync byte 255 buffer2 VAR Byte ' Command buffer3 VAR Byte ' Argument 1 (return data 1) buffer4 VAR Byte ' Argument 2 (return data 2) buffer VAR buffer1 ' For standard array indexing duration VAR Byte ' 50ths of ms duration frequency VAR Byte ' 50ths of frequency pointer VAR Byte ' EEPROM pointer routine VAR Nib ' Routine selector counter VAR Nib index VAR Nib tempBit VAR Bit heartbeat VAR Word ' counter for heartbeat ' -----[ Initialization ]----------------------------------------------------- ' This code makes it possible to toggle the Boe-Bot on/off by pressing and ' releasing the Board of Education's Reset button. Program_Start: FREQOUT 4, 75, 3000 Reset: LOW 15 'Wait for the BT radio to be ready. PAUSE 1000 'Wait for the Bluetooth connection to be established. (eb500 only) 'DO UNTIL IN5 = 1: LOOP HIGH 15 ' turn on led on pin 15 to indicate that program is running tLeft = 758 ' 750+8 bias tRight = 758 heartbeat = 0 ' -----[ Main Routine ]------------------------------------------------------- DO Resume: ' If Message not rcvd, try again 'IF IN5 = 0 THEN Reset ' EB500 disconnected? PULSOUT 13, tLeft ' Servo control pulses PULSOUT 12, tRight heartbeat=heartbeat+1 ' set heartbeat to slightly higher than PC will request a 2 second motor setting IF heartbeat>150 THEN GOTO Reset ENDIF SERIN RX, 84, 20, Resume, [buffer1, buffer2, buffer3, buffer4] ' Get next command heartbeat=0 IF buffer1 <> 255 THEN ' Check for Sync char GOTO Resume ENDIF ON buffer2 GOSUB Set_Servo_Speed, Maneuver, Get_Ir, Get_Whiskers, Get_Pin_States, Speaker_Tone, Set_Pins, Request_Id SEROUT TX, 84, [STR buffer \4] LOOP ' -----[ Subroutine Set_Servo_Speed ]----------------------------------------- ' Range of 0 to 200 with 100 = stopped maps to 650 to 850 with 750 stopped. Set_Servo_Speed: tLeft = buffer3 + 650 ' Decode servo speed. tRight = buffer4 + 650 RETURN ' -----[ Subroutine Speaker_Tone ]-------------------------------------------- Speaker_Tone: duration = buffer3 ' Decode speaker tone frequency = buffer4 FREQOUT 4, duration * 50, frequency * 50 ' Transmit tone RETURN ' Go to resume routine ' -----[ Subroutine Get_Ir ]-------------------------------------------------- ' IR object detection for buf[3] bits 1 (left) and 0 (right). Get_Ir: FREQOUT 9, 1, 38500 ' IR headlight tempBit = IN10 ' IR receiver buffer3.BIT1 = tempBit ' Left IR reply bit FREQOUT 3, 1, 38500 tempBit = IN2 buffer3.BIT0 = tempBit RETURN ' -----[ Subroutine - Check_Whiskers ]---------------------------------------- ' Stores left and right wiskers (contact switches) for buf[3] bit 3 (left) and ' 2 (right). Get_Whiskers: 'IF FlagWhiskers THEN buffer3.BIT3 = IN7 ' Left whisker buffer3.BIT2 = IN8 ' Right whisker 'ENDIF RETURN ' -----[ Subroutine - Get_Pin_States ]------------------------------------- ' Returns all I/O pin states. MSRS has to select the bit it wants. Get_Pin_States: buffer3 = INL ' P0..P7 buffer4 = INH ' P8..P16 RETURN ' -----[ Subroutine - Set_Pins ]---------------------------------------------- ' Sets up to two pins. buf[3] high nibble specifies the operation, and the ' low nibble specifies the pin. The same applies to buf[4]. ' High nibble: ' 0 - no action ' 1 - set to output ' 2 - set to input ' 3 - set to (output-high) ' 4 - set to (output-low) ' Low Nibble: ' 0 to 15 - Specifies I/O pin. ' DO NOT try to set 0, 1, 5, or 6. Set_Pins: temp.NIB1 = buffer3.HIGHNIB temp.NIB0 = buffer3.LOWNIB GOSUB Op_Pins temp.NIB1 = buffer4.HIGHNIB temp.NIB0 = buffer4.LOWNIB GOSUB Op_Pins RETURN Op_pins: SELECT temp.NIB1 CASE 1 OUTPUT temp.NIB0 CASE 2 INPUT temp.NIB0 CASE 3 HIGH temp.NIB0 CASE 4 LOW temp.NIB0 ENDSELECT RETURN ' -----[ Subroutine - Maneuver ]---------------------------------------------- ' Preprogrammed maneuver example. Current setup allows: ' "U" - Back up then U-turn ' "R" - Back up then right turn ' "L" - Back up then left turn Maneuver: FOR temp = 0 TO 35 PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20 NEXT SELECT buffer3 CASE "U" FOR temp = 0 TO 40 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT CASE "R" FOR temp = 0 TO 20 PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 20 NEXT CASE "L" FOR temp = 0 TO 20 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT ENDSELECT RETURN ' -----[ Subroutine - Request_Id ]------------------------------------- ' Returns buffer3 and buffer4 with BB to identify this as a BoeBot. Request_Id: buffer3 = 66 ' P0..P7 buffer4 = 66 ' P8..P16 FREQOUT 4, 75, 3000 RETURN