' {$STAMP BS2}
' {$PBASIC 2.5}

'Χαρτης θυρων εισοδων-εξοδων του Dea Robot
'P0  --free--
'P1  --free--
'P2  --free--
'P3  distancefront sensor
'P4  --free--
'P5  distanceback sensor
'P6  RC Front
'P7  RC Back
'P8  RC Left
'P9  RC Right
'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
rotarefix      VAR     Word      ' 5 = center direction
RCFront        VAR     IN6
RCBack         VAR     IN7
RCLeft         VAR     IN8
RCRight        VAR     IN9



'------start program-------
rotarefix = 6
offset = CenterPulse
PAUSE 2000
GOSUB measure_incl
SetPoint = distanceback  

DO
GOSUB measure_incl
GOSUB CALC_feedback
GOSUB Rc_control
GOSUB Send_Pulse
LOOP

'-------Subroutines--------

CALC_feedback:
  ' Calculate output
  error = SetPoint - distanceback
  p = Kp * error

RETURN

measure_incl:
 'HIGH 3
 'PAUSE 1
 'RCTIME 3,1,distancefront
  HIGH 5
  PAUSE 1
  RCTIME 5,1,distanceback
RETURN

Rc_control:
' check the forward and back controls
  IF (RCFront = 0) THEN p = p + 65      'Lean forward
  IF (RCBack = 0) THEN  p = p - 15      'Lean backward

  ' check the left and right controls
  rotarefix = 5
  IF (RCLeft = 0) THEN  rotarefix = -50    'rotare left
  IF (RCRight = 0) THEN  rotarefix = 55    'rotare right
RETURN

Send_Pulse:
  pulseLeft = p + offset - rotarefix
  pulseright = -p + offset

  PULSOUT Leftservo, pulseright
  PULSOUT Rightservo, pulseLeft
RETURN 