' {$STAMP BS2}
' {$PBASIC 2.5}

'BS2 I/O ports map
'P0  --free--
'P1  --free--
'P2  --free--
'P3  distancefront sensor
'P4  --free--
'P5  distanceback sensor
'P6  --free--
'P7  --free--
'P8  --free--
'P9  --free--
'P10  --free--
'P11  --free--
'P12  Left servo CR
'P13  Right servo CR
'P14  --free--
'P15  --free--

Leftservo      CON     12
Rightservo     CON     13
distancefront  VAR     Word
distanceback   VAR     Word
pulseLeft      VAR     Word
pulseRight     VAR     Word
CenterPulse    CON     750
offset         VAR     Word
SetPoint       VAR     Byte
Kp             CON     40
error          VAR     Word
p              VAR     Word

'------start program-------

offset = CenterPulse
PAUSE 2000
GOSUB measure_incl
SetPoint = distanceback  

DO
GOSUB measure_incl
GOSUB CALC_feedback
GOSUB Send_Pulse
LOOP

'-------Subroutines--------

CALC_feedback:
  ' Calculate output
  error = SetPoint - distanceback
  p = Kp * error
  pulseLeft = p + offset
  pulseright = -p + offset
RETURN

measure_incl:
 'HIGH 3
 'PAUSE 1
 'RCTIME 3,1,distancefront
  HIGH 5
  PAUSE 1
  RCTIME 5,1,distanceback
RETURN

Send_Pulse:
  PULSOUT Leftservo, pulseright
  PULSOUT Rightservo, pulseLeft
RETURN 