[ create a new paste ] login | about

Link: http://codepad.org/NwH9ELwY    [ raw code | output | fork ]

C, pasted on Jul 12:
' -----[ Title ]--------------------------------------------------------------
' Applied Robotics with the SumoBot - SumoWrestler.bs2
' SumoWrestler.bs2 modified so that each state is contained by a
' subroutine.

' {$STAMP BS2}                               ' Target = BASIC Stamp 2
' {$PBASIC 2.5}                              ' Language = PBASIC 2.5

' -----[ I/O Definitions ]---------------------------------------------------

ServoLeft      PIN     13                    ' Left servo connected to P13
ServoRight     PIN     12                    ' Right servo connected to P12

qtiPwrLeft     PIN     10                    ' Left QTI on/off pin P10
qtiSigLeft     PIN     9                     ' Left QTI signal pin P9

qtiPwrRight    PIN     7                     ' Right QTI on/off pin P7
qtiSigRight    PIN     8                     ' Right QTI signal pin P8

DummyPin       PIN     6                     ' I/O pin for pulse-decay P6

LedSpeaker     PIN     5                     ' LED & speaker connected to P5

IrLedLS        PIN     2                     ' Left IR LED connected to P2
IrSenseLS      PIN     1                     ' Left IR detector to P1

IrLedLF        PIN     4                     ' Left IR LED connected to P4
IrSenseLF      PIN     11                    ' Left IR detector to P11

IrLedRF        PIN     15                    ' Right IR LED connected to P15
IrSenseRF      PIN     14                    ' Right IR detector to P14

IrLedRS        PIN     3                     ' Right IR LED connected to P3
IrSenseRS      PIN     0                     ' Right IR detector to P0

' -----[ Constants ]----------------------------------------------------------

' SumoBot maneuvers

Forward        CON     0                     ' Forward
Backward       CON     1                     ' Backward
RotateLeft     CON     2                     ' RotateLeft
RotateRight    CON     3                     ' RotateRight
PivotLeft      CON     4                     ' Pivot to the left
PivotRight     CON     5                     ' Pivot to the right
CurveLeft      CON     6                     ' Curve to the left
CurveRight     CON     7                     ' Curve to the right

' Servo pulse width rotations

FS_CCW         CON     850                   ' Full speed counterclockwise
FS_CW          CON     650                   ' Full speed clockwise
NO_ROT         CON     750                   ' No rotation
LS_CCW         CON     770                   ' Low speed counterclockwise
LS_CW          CON     730                   ' Low speed clockwise

' IR object detectors

IrFreq         CON     38500                 ' IR LED frequency

' -----[ Variables ]----------------------------------------------------------

temp           VAR     Word                  ' Temporary variable
multi          VAR     Word                  ' Multipurpose variable
counter        VAR     Byte                  ' Loop counting variable.

maneuver       VAR     Nib                   ' SumoBot travel maneuver

sensors        VAR     Byte                  ' Sensor flags byte

qtiLF          VAR     sensors.BIT5          ' Stores snapshot of QtiSigLeft
qtiRF          VAR     sensors.BIT4          ' Stores snapshot of QtiSigRight

irLS           VAR     sensors.BIT3          ' State of Left Side IR
irLF           VAR     sensors.BIT2          ' State of Left Front IR
irRF           VAR     sensors.BIT1          ' State of Right Front IR
irRS           VAR     sensors.BIT0          ' State of Right Side IR

' -----[ EEPROM Data ]--------------------------------------------------------

RunStatus      DATA    0                     ' Run status EEPROM byte
QtiThresh      DATA    Word 0                ' Word for QTI threshold time

' -----[ Initialization ]-----------------------------------------------------

GOSUB Reset                                  ' Wait for Reset press/release
GOSUB Start_Delay                            ' 5 Second delay
GOSUB Calibrate_Qtis                         ' Determine b/w threshold
GOSUB Look_About                             ' Was Goto Look_About

' -----[ Main Routine ]-------------------------------------------------------

DO

  IF qtiLF = 1 THEN                          ' Left qti sees line?
    GOSUB Avoid_Tawara_Left                  ' State = avoid left tawara
  ELSEIF qtiRF = 1 THEN                      ' Right qti sees line?
    GOSUB Avoid_Tawara_Right                 ' State = avoid right tawara
  ELSEIF irLF = 1 AND irRF = 1 THEN          ' Both? Lunge forward
    GOSUB Go_Forward                         ' State = Go forward
  ELSEIF irLF = 1 THEN                       ' Just left?
    GOSUB Track_Front_Left_Object            ' State = Track front left obj.
  ELSEIF irRF = 1 THEN                       ' Just right?
    GOSUB Track_Front_Right_Object           ' State = Track front right obj.
  ELSEIF irLS = 1 THEN                       ' Left side?
    GOSUB Track_Side_Left_Object             ' State = track side left obj.
  ELSEIF irRS = 1 THEN                       ' Right side?
    GOSUB Track_Side_Right_Object            ' State = track side right obj.
  ELSE                                       ' Nothing sensed?
    GOSUB Search_Pattern                     ' State = Search pattern
  ENDIF

LOOP

' -----[ Subroutine - Reset ]-------------------------------------------------

Reset:

  READ RunStatus, temp                       ' Byte @RunStatus -> temp
  temp = temp + 1                            ' Increment temp
  WRITE RunStatus, temp                      ' Store new value for next time

  IF (temp.BIT0 = 1) THEN                    ' Examine temp.BIT0
    DEBUG CLS, "Press/release Reset", CR,    ' 1 -> end, 0 -> keep going
               "button..."
    END
  ELSE
    DEBUG CR, "Program running..."
  ENDIF

  RETURN

' -----[ Subroutine - Start_Delay ]-------------------------------------------

Start_Delay:

  FOR counter = 1 TO 5                       ' 5 beeps, 1/second
    PAUSE 900
    FREQOUT LedSpeaker, 100, 3000
  NEXT

  RETURN

' -----[ Subroutine - Calibrate_Qtis ]----------------------------------------

Calibrate_Qtis:

  HIGH qtiPwrLeft                            ' Turn left QTI on
  HIGH qtiSigLeft                            ' Discharge capacitor
  PAUSE 1

  RCTIME qtiSigLeft, 1, temp                 ' Measure charge time

  LOW qtiPwrLeft                             ' Turn left QTI off
  multi = temp                               ' Free temp for another RCTIME

  HIGH qtiPwrRight                           ' Turn right QTI on
  HIGH qtiSigRight                           ' Discharge capacitor
  PAUSE 1
  RCTIME qtiSigRight, 1, temp                ' Measure charge time

  multi = (multi + temp) / 2                 ' Calculate average

  multi = multi / 4                          ' Take 1/4 average

  IF multi > 220 THEN                        ' Account for code overhead
    multi = multi - 220
  ELSE
    multi = 0
  ENDIF

  WRITE QtiThresh, Word multi                ' Threshold to EEPROM

  RETURN

' -----[ Subroutine - Servos_And_Sensors ]------------------------------------

Servos_And_Sensors:

  GOSUB Pulse_Servos                         ' Call Pulse_Servos subroutine

  ' Call sensor subroutine(s).

  sensors = 0                                ' Clear previous sensor values

  GOSUB Read_Object_Detectors                ' Call Read_Object_Detectors
  GOSUB Read_Line_Sensors                    ' Look for lines

  RETURN

' -----[ Subroutine - Pulse_Servos ]------------------------------------------

Pulse_Servos:

  ' Pulse to left servo
  LOOKUP maneuver, [ FS_CCW, FS_CW, FS_CW, FS_CCW,
                     NO_ROT, FS_CCW, LS_CCW, FS_CCW ], temp
  PULSOUT ServoLeft, temp

  ' Pulse to right servo
  LOOKUP maneuver, [ FS_CW, FS_CCW, FS_CW, FS_CCW,
                     FS_CW, NO_ROT, FS_CW, LS_CW ], temp
  PULSOUT ServoRight, temp

  ' Pause between pulses (remove when using IR object detectors + QTIs).
  ' PAUSE 20

  RETURN

' -----[ Subroutine - Read_Object_Detectors ]---------------------------------

Read_Object_Detectors:

  FREQOUT IrLedRS, 1, IrFreq                 ' Right side IR LED headlight
  irRS = ~IrSenseRS                          ' Save right side IR receiver

  FREQOUT IrLedRF, 1, IrFreq                 ' Repeat for right-front
  irRF = ~IrSenseRF

  FREQOUT IrLedLF, 1, IrFreq                 ' Repeat for left-front
  irLF = ~IrSenseLF

  FREQOUT IrLedLS, 1, IrFreq                 ' Repeat for left side
  irLS = ~IrSenseLS

  RETURN

' -----[ Subroutine - Read_Line_Sensors ]-------------------------------------

Read_Line_Sensors:

  HIGH qtiPwrLeft                            ' Turn on QTIs
  HIGH qtiPwrRight
  HIGH qtiSigLeft                            ' Push signal voltages to 5 V
  HIGH qtiSigRight
  PAUSE 1                                    ' Wait 1 ms for capacitors

  READ QtiThresh, Word temp                  ' Get threshold time

  INPUT qtiSigLeft                           ' Start the decays
  INPUT qtiSigRight

  PULSOUT DummyPin, temp                     ' Wait threshold time

  qtiLF  = ~qtiSigLeft                       ' Snapshot of QTI signal states
  qtiRF = ~qtiSigRight

  LOW qtiPwrLeft                             ' Turn off QTIS
  LOW qtiPwrRight

  RETURN

' -----[ Subroutine - Avoid_Tawara_Left ]-------------------------------------

Avoid_Tawara_Left:

  FOR counter = 1 TO 15                      ' Back up
    maneuver = Backward
    GOSUB Servos_And_Sensors
  NEXT
  FOR counter = 1 TO 15                      ' Turn right
    maneuver = RotateRight
    GOSUB Servos_And_Sensors
  NEXT

  RETURN

' -----[ Subroutine - Avoid_Tawara_Right ]------------------------------------

Avoid_Tawara_Right:

  FOR counter = 1 TO 15                      ' Back up
    maneuver = Backward
    GOSUB Servos_And_Sensors
  NEXT
  FOR counter = 1 TO 15                      ' Turn left
    maneuver = RotateLeft
    GOSUB Servos_And_Sensors
  NEXT

  RETURN

' -----[ Subroutine - Go_Forward ]--------------------------------------------

Go_Forward:

  maneuver = Forward                         ' 1 forward pulse
  GOSUB Servos_And_Sensors

  RETURN

' -----[ Subroutine - Track_Front_Left_Object ]-------------------------------

Track_Front_Left_Object:

  counter = 0
  DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
    maneuver = CurveLeft                     ' Curve left 15
    GOSUB Servos_And_Sensors
    counter = counter + 1
  LOOP
  DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30
    maneuver = RotateLeft                    ' Rotate left 30
    GOSUB Servos_And_Sensors
    counter = counter + 1
  LOOP

  RETURN

' -----[ Subroutine - Track_Front_Right_Object ]------------------------------

Track_Front_Right_Object:

  counter = 0
  DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
    maneuver = CurveRight                    ' Curve right 15
    GOSUB Servos_And_Sensors
    counter = counter + 1
  LOOP
  DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30
    maneuver = RotateRight                   ' Rotate right 30
    GOSUB Servos_And_Sensors
    counter = counter + 1
  LOOP

  RETURN

' -----[ Subroutine - Track_Side_Left_Object ]--------------------------------

Track_Side_Left_Object:

  DO UNTIL irRF = 1 OR irLF = 1              ' Rotate left
    maneuver = RotateLeft
    GOSUB Servos_And_Sensors
  LOOP

  RETURN

' -----[ Subroutine - Track_Side_Right_Object ]-------------------------------

Track_Side_Right_Object:

  DO UNTIL irRF = 1 OR irLF = 1              ' Rotate right
    maneuver = RotateRight
    GOSUB Servos_And_Sensors
  LOOP

  RETURN

' -----[ Subroutine - Search_Pattern ]----------------------------------------

Search_Pattern:

  FOR counter = 1 TO 35                      ' and watch all sensors
    maneuver = Forward                       ' Forward
    GOSUB Servos_And_Sensors
    IF sensors <> 0 THEN GOTO Next_State
  NEXT
  Look_About:
  FOR counter = 1 TO 12                      ' Look right
    maneuver = RotateRight
    GOSUB Servos_And_Sensors
    IF sensors <> 0 THEN GOTO Next_State
  NEXT
  FOR counter = 1 TO 24                      ' Look left
    maneuver = RotateLeft
    GOSUB Servos_And_Sensors
    IF sensors <> 0 THEN GOTO Next_State
  NEXT
  FOR counter = 1 TO 12                      ' Re-align to forward
    maneuver = RotateRight
    GOSUB Servos_And_Sensors
    IF sensors <> 0 THEN GOTO Next_State
  NEXT

  Next_State:                                ' Exit point of search pattern

  RETURN


Output:
Line 1: error: missing terminating ' character
Line 2: error: missing terminating ' character
Line 3: error: missing terminating ' character
Line 4: error: missing terminating ' character
Line 0: warning: character constant too long for its type
Line 6: error: expected identifier or '(' before '\x20202020'
Line 0: warning: character constant too long for its type
Line 9: error: missing terminating ' character
Line 11: error: missing terminating ' character
Line 12: error: missing terminating ' character
Line 14: error: missing terminating ' character
Line 15: error: missing terminating ' character
Line 17: error: missing terminating ' character
Line 18: error: missing terminating ' character
Line 20: error: missing terminating ' character
Line 22: error: missing terminating ' character
Line 24: error: missing terminating ' character
Line 25: error: missing terminating ' character
Line 27: error: missing terminating ' character
Line 28: error: missing terminating ' character
Line 30: error: missing terminating ' character
Line 31: error: missing terminating ' character
Line 33: error: missing terminating ' character
Line 34: error: missing terminating ' character
Line 36: error: missing terminating ' character
Line 38: error: missing terminating ' character
Line 40: error: missing terminating ' character
Line 41: error: missing terminating ' character
Line 42: error: missing terminating ' character
Line 43: error: missing terminating ' character
Line 44: error: missing terminating ' character
Line 45: error: missing terminating ' character
Line 46: error: missing terminating ' character
Line 47: error: missing terminating ' character
Line 49: error: missing terminating ' character
Line 51: error: missing terminating ' character
Line 52: error: missing terminating ' character
Line 53: error: missing terminating ' character
Line 54: error: missing terminating ' character
Line 55: error: missing terminating ' character
Line 57: error: missing terminating ' character
Line 59: error: missing terminating ' character
Line 61: error: missing terminating ' character
Line 63: error: missing terminating ' character
Line 64: error: missing terminating ' character
Line 65: error: missing terminating ' character
Line 67: error: missing terminating ' character
Line 69: error: missing terminating ' character
Line 71: error: missing terminating ' character
Line 72: error: missing terminating ' character
Line 74: error: missing terminating ' character
Line 75: error: missing terminating ' character
Line 76: error: missing terminating ' character
Line 77: error: missing terminating ' character
Line 79: error: missing terminating ' character
Line 81: error: missing terminating ' character
Line 82: error: missing terminating ' character
Line 84: error: missing terminating ' character
Line 86: error: missing terminating ' character
Line 87: error: missing terminating ' character
Line 88: error: missing terminating ' character
Line 89: error: missing terminating ' character
Line 91: error: missing terminating ' character
Line 95: error: missing terminating ' character
Line 96: error: missing terminating ' character
Line 97: error: missing terminating ' character
Line 98: error: missing terminating ' character
Line 99: error: missing terminating ' character
Line 100: error: missing terminating ' character
Line 101: error: missing terminating ' character
Line 102: error: missing terminating ' character
Line 103: error: missing terminating ' character
Line 104: error: missing terminating ' character
Line 105: error: missing terminating ' character
Line 106: error: missing terminating ' character
Line 107: error: missing terminating ' character
Line 108: error: missing terminating ' character
Line 109: error: missing terminating ' character
Line 110: error: missing terminating ' character
Line 115: error: missing terminating ' character
Line 119: error: missing terminating ' character
Line 120: error: missing terminating ' character
Line 121: error: missing terminating ' character
Line 123: error: missing terminating ' character
Line 124: error: missing terminating ' character
Line 133: error: missing terminating ' character
Line 137: error: missing terminating ' character
Line 144: error: missing terminating ' character
Line 148: error: missing terminating ' character
Line 149: error: missing terminating ' character
Line 152: error: missing terminating ' character
Line 154: error: missing terminating ' character
Line 155: error: missing terminating ' character
Line 157: error: missing terminating ' character
Line 158: error: missing terminating ' character
Line 160: error: missing terminating ' character
Line 162: error: missing terminating ' character
Line 164: error: missing terminating ' character
Line 166: error: missing terminating ' character
Line 172: error: missing terminating ' character
Line 176: error: missing terminating ' character
Line 180: error: missing terminating ' character
Line 182: error: missing terminating ' character
Line 184: error: missing terminating ' character
Line 186: error: missing terminating ' character
Line 187: error: missing terminating ' character
Line 191: error: missing terminating ' character
Line 195: error: missing terminating ' character
Line 200: error: missing terminating ' character
Line 205: error: missing terminating ' character
Line 206: error: missing terminating ' character
Line 210: error: missing terminating ' character
Line 214: error: missing terminating ' character
Line 215: error: missing terminating ' character
Line 217: error: missing terminating ' character
Line 220: error: missing terminating ' character
Line 223: error: missing terminating ' character
Line 228: error: missing terminating ' character
Line 232: error: missing terminating ' character
Line 234: error: missing terminating ' character
Line 236: error: missing terminating ' character
Line 238: error: missing terminating ' character
Line 240: error: missing terminating ' character
Line 243: error: missing terminating ' character
Line 245: error: missing terminating ' character
Line 248: error: missing terminating ' character
Line 253: error: missing terminating ' character
Line 257: error: missing terminating ' character
Line 261: error: missing terminating ' character
Line 268: error: missing terminating ' character
Line 272: error: missing terminating ' character
Line 276: error: missing terminating ' character
Line 283: error: missing terminating ' character
Line 287: error: missing terminating ' character
Line 292: error: missing terminating ' character
Line 298: error: missing terminating ' character
Line 303: error: missing terminating ' character
Line 310: error: missing terminating ' character
Line 316: error: missing terminating ' character
Line 321: error: missing terminating ' character
Line 328: error: missing terminating ' character
Line 332: error: missing terminating ' character
Line 339: error: missing terminating ' character
Line 343: error: missing terminating ' character
Line 350: error: missing terminating ' character
Line 354: error: missing terminating ' character
Line 355: error: missing terminating ' character
Line 360: error: missing terminating ' character
Line 365: error: missing terminating ' character
Line 370: error: missing terminating ' character
Line 376: error: missing terminating ' character


Create a new paste based on this one


Comments: