'{$STAMP BS2}
'{$PBASIC 2.5}

'---------[Time constants for BASIC Stamp 2]------------------------------
NULL      CON 750       'Pulse width for 1.5ms pulse.
SCALE     CON $100      '256 * amount to scale pulses by.

'--------[Calibration Data]-----------------------------------------------
'DATA @0, 7,10,12,14,15,17,20,22
'DATA     25,27,30,34,37,42,49,69
'DATA     7,10,11,13,15,17,19,22
'DATA     24,27,30,34,37,42,48,59
'DATA     4,6,9,11,12,14,17,19
'DATA     21,24,27,31,35,41,51,77
'DATA     9,11,13,15,17,19,22,25
'DATA     27,30,33,37,41,47,54,72

'--------[Calibration Constants]-----------------------------------------------
FULLROT   CON  $46FF
BRDSPER   CON  $039B
SDIRINC   CON  $5AA7

'---------[Other Global Constants]---------------------------------------------

RIGHT     CON 0         'Constants used as subscripts into bit arrays.
LEFT      CON 1
FWD       CON 0
BAK       CON 1

SenseR    PIN 1        'Lefthand encoder input.
SenseL    PIN 2        'Righthand encoder input. (MUST be SenseR + 1.)

MotorR    PIN 12        'Lefthand motor output.
MotorL    PIN 13        'Righthand motor output. (MUST be MotorR + 1.)

Sense     CON SenseR    'Base address for encoders.
Motor     CON MotorR    'Base address for motors.

Arg       VAR Byte      'Requested travel or turn amount.
Arg_2      VAR Byte      'Requested turn amount for performing PID angle adjustment
Veloc     VAR Nib       'Requested maximum velocity.

DirErr    VAR Word      'Current directional error in brads and 1/256 brads.

Prev      VAR Bit(2)    'Previous readings from encoders.
New       VAR Bit(2)    'Current readings from encoders.
Dir       VAR Bit(2)    'Wheel directions (FWD or BAK).
Side      VAR Bit       'Side index (RIGHT or LEFT).
Opp       VAR Bit       'Index to the other side (saves code).

Pulse     VAR Byte      'Current unscaled, unnulled servo pulse value.

Dist      VAR Byte(2)   'Distance for each wheel to travel.
Counts    VAR Byte(2)   'Encoder pulse countdown for each wheel.
i         VAR Byte      'General FOR loop index.
m         VAR Byte      'Another FOR loop index.
n         VAR Byte      'Yet another FOR loop index.

Angle     VAR Word(2)   'Angle read from the compass.
GetAngle  VAR Bit       'Index for reading angle.
Setpoint  CON 0         'Index for Setpoint of the compass bearing.
Bearing   CON 1         'Index for Current compass bearing.
AbsErr    VAR Word      'Absolute angle error

'---------[I2C Global Constants]---------------------------------------------
SDA       CON  8      ' I2C data
SCL       CON  9      ' I2C clock
SDAin     VAR  IN8
SDAout    VAR  OUT8
SDAdir    VAR  DIR8

I2cBuf    VAR  Byte      ' I2c read/write buffer
I2cAddr   VAR  Byte      ' Address of I2C device
I2cReg    VAR  Byte      ' Register number within I2C device
I2cData   VAR  Word      ' Data to read/write
I2cAck    VAR  Bit      ' Acknowledge bit


'=========[MAIN PROGRAM]=======================================================
PAUSE 3000

  GetAngle = Setpoint : GOSUB GetBearing
  GetAngle = Bearing : GOSUB GetBearing
  DEBUG 2,0,4, "Angle(Bearing): " , DEC3 Angle(Bearing)
  DEBUG 2,0,5, "Angle(Setpoint): ", DEC3 Angle(Setpoint)
  PAUSE 250

  Arg_2 = 12   'total distance to travel

'---- Split the total distance into steps ----------------------------
FOR n = 1 TO (Arg_2/4)
DEBUG 2, 0,1 , " n: ", DEC n, CR

'---- Perform angle adjustment X2 --------------------------------------
FOR m = 1 TO 2
DEBUG 2, 0,2 , " m: ", DEC m, CR
  GetAngle = Bearing : GOSUB GetBearing
  GOSUB CheckError
  DEBUG 2,0,4, "Angle(Bearing): " , DEC3 Angle(Bearing)
  DEBUG 2,0,5, "Angle(Setpoint): ", DEC3 Angle(Setpoint)
  PAUSE 250

  IF ( AbsErr >= 1 ) THEN
    DEBUG 2,0,6, "Peforming PID..."
      IF ( Angle(Bearing) > Angle(Setpoint) ) THEN
      SELECT ( Angle(Bearing) - Angle(Setpoint) )
        CASE > 128
          DEBUG 2,0,8 ,  "Turn Right 1 " , CR
          Arg = 256 - ABS(Angle(Bearing)) + ABS(Angle(Setpoint))
          DEBUG 2,0,9, "Turn Arg. : " , DEC3 Arg
          Arg = - (256 - ABS(Angle(Bearing)) + ABS(Angle(Setpoint)))
          GOSUB Turn
        CASE <= 128
          DEBUG 2,0,8 ,  "Turn Left 1 " , CR
          Arg = ABS(Angle(Bearing)) - ABS(Angle(Setpoint))
          DEBUG 2,0,9, "Turn Arg. : " , DEC3 Arg
          GOSUB Turn
        CASE ELSE
        DEBUG 2,0,7, "Bad Command at 'Turn' 1", CR
      ENDSELECT
      ELSE
      SELECT ( Angle(Setpoint) - Angle(Bearing) )
        CASE > 128
          DEBUG 2,0,8 ,  "Turn Left 2 " , CR
          Arg = 256 - ABS(Angle(Setpoint)) + ABS(Angle(Bearing))
          DEBUG 2,0,9, "Turn Arg. : " , DEC3 Arg
          GOSUB Turn
        CASE <= 128
          DEBUG 2,0,8 ,  "Turn Right 2 " , CR
          Arg = ABS(Angle(Setpoint)) - ABS(Angle(Bearing))
          DEBUG 2,0,9, "Turn Arg. : " , DEC3 Arg
          Arg = -(ABS(Angle(Setpoint)) - ABS(Angle(Bearing)))
          GOSUB Turn
        CASE ELSE
        DEBUG 2,0,7, "Bad Command at 'Turn' 2", CR
      ENDSELECT
      ENDIF
  ENDIF
  PAUSE 250

  GetAngle = Bearing : GOSUB GetBearing
  GOSUB CheckError
  DEBUG 2,0,4, "Angle(Bearing): " , DEC3 Angle(Bearing)
  DEBUG 2,0,5, "Angle(Setpoint): ", DEC3 Angle(Setpoint)
  PAUSE 250

NEXT
'---- End: Perform angle adjustment ------------------------------------
PAUSE 100
  DEBUG 2, 0,14 , "Moving..." , CR
  Veloc = 10 : Arg = 4 : GOSUB Move
NEXT
'---- End: Split the total distance into steps -------------------------

END

'=========[SUBROUTINES]========================================================

'---------[Move]---------------------------------------------------------------

'Move the robot forward/backward by signed byte Arg at unsigned speed Veloc.

Move:
  Dir(LEFT) = Arg.BIT7      'Direction of motion given by sign bit of Arg.
  Dir(RIGHT) = Dir(LEFT)     'Both directions are the same.
  Arg = ABS(Arg << 8) >> 8   'Take the absolute value of Arg to get distance.
                             'Compute length of first leg of tack.
  Dist(LEFT) = Arg * (16 - (ABS(DirErr) / (BRDSPER >> 4))) / 16

  IF (Dist(LEFT)) THEN       'Is it greater than zero?
    Dist(RIGHT) = Dist(LEFT) '  Yes: Right wheel goes the same distance.
    GOSUB DoMove             '       Execute the move
    Arg = Arg - Dist(LEFT)   '       Subtract the distance moved from Arg.
    IF (Arg) THEN            '       Anything left?
      Dist(LEFT) = DirErr.BIT15     '  Yes: Correct direction by the sign of DirErr.
      Dist(RIGHT) = Dist(LEFT) ^ 1  '       Only one wheel moves forward.
      GOSUB DoMove                  '       Execute the move.
    ENDIF
  ENDIF
  IF (Arg) THEN              'Now back to Arg. Is it greater than zero?
    Dist(RIGHT) = Arg         '  Yes: Set distances from it.
    Dist(LEFT) = Arg
    GOSUB DoMove             '       Execute this leg of the move.
  ENDIF
  RETURN

'---------[Turn]---------------------------------------------------------------

'Turn the robot on its axis by the amount in signed byte Arg at speed Veloc.
'Arg > 0 turns left; Arg < 0 turns right.

Turn:
  DirErr = DirErr + (Arg << 8)          'Adjust direction error by amount of turn.
  Arg = ABS(DirErr) ** FULLROT >> 7 + 1 >> 1 'Compute new Arg from new DirErr.
  Dir(RIGHT) = DirErr.BIT15             'Direction of turn is sign bit of DirrErr.
  Arg = ABS(Arg << 8) >> 8              'Amount of turn is absolute value of Arg.
  Dist(LEFT) = Arg >> 1                 'Split turn amount evenly between both wheels.
  Dist(RIGHT) = Arg >> 1 + (Arg.BIT0)   'Round right wheel up if Arg is odd.
  Dir(LEFT) = ~ Dir(RIGHT)              'Left wheel goes opposite direction.
  GOTO DoMove                           'Execute the motion.


'--------[DoMove]--------------------------------------------------------------

'Move right wheel by Dist(RIGHT) in direction Dir(RIGHT) and
'left wheel by Dist(LEFT) in direction Dir(LEFT) at peak velocity Veloc,
'using ramping and RIGHT/LEFT coordination.

DoMove:
  'Correct DirErr by effects of presumptive motion.
  'Initialize Counts TO Dist.
  'Save current encoder status.

  FOR Side = RIGHT TO LEFT
    DirErr = DirErr + ((Side ^ Dir(Side) << 1 - 1) * Dist(Side) * BRDSPER)
    Counts(Side) = Dist(Side)
    Prev(Side) = INS.LOWBIT(Sense + Side)
  NEXT

  'GOSUB GetBearing
  'GOSUB CheckError
  'IF ( AbsErr > 0 ) THEN DoPID 'Perform Angle Adjustment before moving

  'Do for as long as there are encoder counts remaining...
  DO WHILE (Counts(RIGHT) OR Counts(LEFT))

    'Get new encoder state for each wheel.
    'If it's changed, decrement that wheel's Count.
    FOR Side = RIGHT TO LEFT
      New(Side) = INS.LOWBIT(Sense + Side)
      IF (New(Side) <> Prev(Side) AND Counts(Side)) THEN
        Prev(Side) = New(Side)
        Counts(Side) = Counts(Side) - 1
      ENDIF
    NEXT

    'For each wheel decide whether and how much to pulse its servo.
    FOR Side = RIGHT TO LEFT
      Opp = ~ Side
      IF (Counts(Side) AND Counts(Side) * Dist(Opp) + (Dist(Side)) >= Counts(Opp) * Dist(Side) + (Dist(Opp) >> 1)) THEN
        Pulse = (Veloc MIN 3) MAX ((Counts(Side) MIN Counts(Opp)) MAX ((Dist(Side) - Counts(Side)) MIN (Dist(Opp) - Counts(Opp))) << 1 MIN 3)
        READ Side << 1 + (Dir(Side) ^ Side) << 4 + (Pulse * Dist(Side) / (Dist(Side) MIN Dist(Opp)) + 1 MAX 15), Pulse
        PULSOUT Motor + Side, NULL - ((Dir(Side) ^ Opp << 1 - 1) * Pulse)
      ENDIF
    NEXT

    'Pause between pulses.
    PAUSE 5
  LOOP
RETURN


'--------[Check Error]---------------------------------------------------------
CheckError:
  'Check the Bearing before move
  IF (Angle(Bearing) > Angle(Setpoint)) THEN
    AbsErr = ABS ( Angle(Bearing) - Angle(Setpoint) )
  ELSE
    AbsErr = ABS ( Angle(Setpoint) - Angle(Bearing) )
  ENDIF
  DEBUG 2,0,10, "AbsErr : ", DEC3 AbsErr
RETURN


'--------[Compass I2C Bus Subroutines]-----------------------------------------
GetBearing:
  I2cAddr = $c0      ' Compass module address
  I2cReg = 1        ' Bearing as 0-255 (BRAD)
  HIGH  SDA
  HIGH  SCL
  LOW  SDA
  LOW  SCL
  I2cBuf = I2cAddr
  SHIFTOUT SDA, SCL, MSBFIRST, [I2cBuf]
  INPUT  SDA
  HIGH  SCL          ' clock in the ack' bit
  LOW  SCL
  I2cBuf = I2cReg
  SHIFTOUT SDA, SCL, MSBFIRST, [I2cBuf]
  INPUT  SDA
  HIGH  SCL          ' clock in the ack' bit
  LOW  SCL
  HIGH  SDA
  HIGH  SCL
  LOW  SDA
  LOW  SCL
  I2cBuf = I2cAddr | 1
  SHIFTOUT SDA, SCL, MSBFIRST, [I2cBuf]
  INPUT  SDA
  HIGH  SCL          ' clock in the ack' bit
  LOW  SCL
  I2cAck = 0        ' send Nak
  SHIFTIN SDA, SCL, MSBPRE, [I2cBuf]
  SDAout = 0
  SDAdir = I2cAck
  HIGH  SCL          ' clock out the ack' bit
  LOW   SCL
  INPUT  SDA
  I2cData.LOWBYTE = I2cBuf  ' read the data
  I2cData.HIGHBYTE = 0
  LOW  SDA
  HIGH  SCL
  HIGH  SDA

  IF GetAngle = 0 THEN
    Angle(Setpoint) = I2cData
  ELSE
    Angle(Bearing) = I2cData
  ENDIF

  'DEBUG  2,0,0, "Compass Bearing (0-255 BRAD)   ", DEC3 I2cData
  'DEBUG 2,0,2, "Bearing : ", DEC3 Angle(Bearing)
RETURN