[Greg Ross]

Slot 4 (BB2003_AUTON_PROG1) contains the first several of our autonomous programs: (BTW, BB2003Include.bsppi is an include file -- which defines all our shared variables, and gets included in all our programs.)

' {$STAMP BS2SX}
' {$PBasic 2.5}

{$Include BB2003Include.bsppi}

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1

get AUTON_PROG_NUM, autonProgNum

select autonProgNum
  case DEAD_IN_THE_WATER_PROG        ' 1
    gosub DeadInTheWater
  case LINE_FOLLOW_LEFT_PROG         ' 2
    gosub FollowTheLineFromRightSide
  case LINE_FOLLOW_RIGHT_PROG        ' 3
    gosub FollowTheLineFromLeftSide
  case DEAD_RECKON_FROM_RIGHT_SIDE   ' 4
    gosub DeadReckonFromRightSide
  case DEAD_RECKON_FROM_LEFT_SIDE    ' 5
    gosub DeadReckonFromLeftSide
  case else
    run BB2003_AUTON_PROG2
endselect

Run BB2003_OUTPUT_DATA




DeadInTheWater:
  'debug "DeadInTheWater", cr
  leftWheelOutput=127
  rightWheelOutput=127

  return





[I'll spare you the line-following code. It's long, involved, hard to
understand, and slow.




DeadReckonFromRightSide:
' Handle dead reckoning timing by using the matchTimer.
' matchTimer is incremented every 1/38.2 seconds.

put PORT1_X, 63 ' open the grippers

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1
{$if (matchTimer < 28)} ' Go straight for 0.75 seconds.
  leftWheelOutput = 254
  rightWheelOutput = 254
{$else if (matchTimer < 28+76)} ' 2 seconds swooping turn to right
  leftWheelOutput = 254
  rightWheelOutput = 127+25

{$else if (matchTimer < 28+76+19)} ' 0.5 seconds to stop
  leftWheelOutput = 127
  rightWheelOutput = 127
  plantRearWedge = 1
  unplantRearWedgeFwd = 0
{$else if (matchTimer < 28+76+19+152)} ' 4 seconds straight up the ramp backwards
  leftWheelOutput = 0
  rightWheelOutput = 0
{$else if (matchTimer < 28+76+19+152+19)} ' 0.5 seconds to stop
  leftWheelOutput = 127
  rightWheelOutput = 127
{$else}
  put PORT1_X, 127 ' Release grippers

  plantRearWedge = 0
  unplantRearWedgeFwd = 1

  highGear = 0
  highGearShiftFwd = highGear
  lowGearShiftFwd = highGear ^ 1
  leftWheelOutput = 127
  rightWheelOutput = 127
{$endif}

return





DeadReckonFromLeftSide:
[This is just the same as the preceding code... except for the
direction of the turns, some slight timing differences and
some compensation for differences between the left and right
sides of the drive train.]

Slot 5 (BB2003_AUTON_PROG2) contains some more autonomous programs:

' {$STAMP BS2SX}
' {$PBasic 2.5}

{$Include BB2003Include.bsppi}

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1

get AUTON_PROG_NUM, autonProgNum

select autonProgNum
  case WAIT_BY_WALL_FROM_RIGHT_SIDE ' 6
    gosub WaitByWallFromRightSide
  case WAIT_BY_WALL_FROM_LEFT_SIDE  ' 7
    gosub WaitByWallFromLeftSide
  case LIMBO_UNDER_BAR_SLOW         ' 8
    gosub LimboUnderBarSlow
  case LIMBO_UNDER_BAR_FAST         ' 9
    gosub LimboUnderBarFast
  case LEARN6 + 100
    'debug "Playing back -- ", dec? matchTimer
    run BB2003_PLAYBACK6
  case LEARN7 + 100
    'debug "Playing back -- ", dec? matchTimer
    run BB2003_PLAYBACK7
  case else
    ' Something's wrong. Go back and do the Mars rock dance.
    put AUTON_PROG_NUM, DEAD_IN_THE_WATER_PROG
    run BB2003_AUTON_PROG1
endselect

Run BB2003_OUTPUT_DATA




WaitByWallFromRightSide:
' Handle dead reckoning timing by using the matchTimer.
' matchTimer is incremented every 1/38.2 seconds.

put PORT1_X, 63 ' open the grippers

' do an arc for the first 6.29 seconds (240 matchTimer counts)
get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1
{$if (matchTimer < 28)} ' 0.75 second straight
  leftWheelOutput = 254
  rightWheelOutput = 254
{$else if (matchTimer < 28+76)} ' 2 seconds swooping turn to right
  leftWheelOutput = 254
  rightWheelOutput = 127+25

{$else}
  put PORT1_X, 127 ' Release grippers

  leftWheelOutput = 127
  rightWheelOutput = 127
{$endif}

return




WaitByWallFromLeftSide:
' Handle dead reckoning timing by using the matchTimer.
' matchTimer is incremented every 1/38.2 seconds.

put PORT1_X, 63 ' open the grippers

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1

{$if (matchTimer < 38)} ' Go straight for 1 second
  leftWheelOutput = 254
  rightWheelOutput = 254
{$else if (matchTimer < 38+76)} ' Swooping turn to right for 2 seconds
  leftWheelOutput = 127+10
  rightWheelOutput = 254
{$else}
  put PORT1_X, 127 ' Release grippers

  leftWheelOutput = 127
  rightWheelOutput = 127
{$endif}

return






LimboUnderBarSlow:

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1

{$if (LiftLowerLimitSwitch = 0)} ' Lower lift to limit switch
  put LIFT_POWER, 127+25 ' Lower the lift
  if LiftLowerLimitSwitch = 1 then
    debug "Lower Limit Reached", cr
    put LIFT_POWER, 127
  endif

  highGear = 0
  highGearShiftFwd = highGear
  lowGearShiftFwd = highGear ^ 1

  plantFrontWedge = 1
  plantRearWedge = 1
  unplantRearWedgeFwd = 0

  leftWheelOutput = 127
  rightWheelOutput = 127
{$else if (matchTimer < 239+114)} ' Stop the lift and Back up under the bar for 3 seconds.
  put LIFT_POWER, 127
  leftWheelOutput = 0
  rightWheelOutput = 0
{$else}
  leftWheelOutput = 127
  rightWheelOutput = 127
{$endif}

return





LimboUnderBarFast:

get MATCH_TIMER_LOW, matchTimer.byte0
get MATCH_TIMER_HIGH, matchTimer.byte1


{$if (LiftLowerLimitSwitch = 0)} ' Lower lift to limit switch
  ' Don't move until it's safe.
  leftWheelOutput = 127
  rightWheelOutput = 127

 ' Lower the lift at half speed.
  put LIFT_POWER, 127+63
  if LiftLowerLimitSwitch = 1 then
    debug "Lower Limit Reached", cr
    put LIFT_POWER, 127
  endif

  ' Make sure we're in high gear ...
  highGear = 1
  highGearShiftFwd = highGear
  lowGearShiftFwd = highGear ^ 1

  ' ... and wedges are down.
  plantFrontWedge = 1
  plantRearWedge = 1
  unplantRearWedgeFwd = 0

{$else if (matchTimer < 239)} ' Stop the lift and Back up under the bar for 3 seconds.
  put LIFT_POWER, 127
  leftWheelOutput = 0
  rightWheelOutput = 0
{$else}
  leftWheelOutput = 127
  rightWheelOutput = 127
{$endif}

return

Slots 6 and 7 (BB2003_PLAYBACK6 and BB2003_PLAYBACK7) are identical, and are used to record 15 seconds worth of driver control to EEPROM for later playback in autonomous mode:

' {$STAMP BS2SX}
' {$PBasic 2.5}

{$Include BB2003Playback.bsppi}

BB2003Playback.bsppi which gets inserted into both slots 6 and 7, contains all the logic to both save and replay the operator commands:

{$Include BB2003Include.bsppi}

' We need to save 6 bytes of data to be able to satisfactorily
' control the robot during playback. By only saving every other
' time through the loop, we are able to save 15.5 seconds worth
' of data.
MOVE_COUNT con 296 ' 15.5 seconds worth of "moves"
MOVE_SAVE_SIZE con 6*MOVE_COUNT
theMoves data (MOVE_SAVE_SIZE) ' Reserve space for the learned moves

theMoveIndex = (matchTimer >> 1) * 6 + theMoves

if Autonomous = 0 then
  if (theMoveIndex < MOVE_SAVE_SIZE) and (matchTimer & 1 = 0) then
    write theMoveIndex+0, leftWheelOutput
    write theMoveIndex+1, relayA
    write theMoveIndex+2, rightWheelOutput
    write theMoveIndex+3, relayB
    write theMoveIndex+4, liftPower
    write theMoveIndex+5, GripperJoyStick
  else
    Run BB2003_OUTPUT_DATA
  endif
else
  if (theMoveIndex < MOVE_SAVE_SIZE) then
    read theMoveIndex+0, leftWheelOutput
    read theMoveIndex+1, relayA
    read theMoveIndex+2, rightWheelOutput
    read theMoveIndex+3, relayB
    read theMoveIndex+4, liftPower
    read theMoveIndex+5, GripperJoyStick
  else
    ' If playback continues longer than we have data for, just
    ' continue playing back the last data we have (except stop the
    ' wheels and the lift.)
    theMoveIndex = MOVE_SAVE_SIZE - 5
    leftWheelOutput = 127
    read theMoveIndex+1, relayA
    rightWheelOutput = 127
    read theMoveIndex+3, relayB
    liftPower = 127
    read theMoveIndex+5, GripperJoyStick
  endif
endif

'debug dec theMoveIndex, ": ",  dec leftWheelOutput, " ",  \
       ibin relayA, " ",  dec rightWheelOutput, " ", \
       ibin relayB, " ",  dec liftPower, " ",  dec GripperJoyStick, cr

Run BB2003_OUTPUT_DATA