;Wham Drag race starter pack 8/2/2020.
#include "Drag_race_include_V2.basinc"  ; needed for all the symbols and subroutines

myprog:
#rem
 see the seperate file  "Student Drag Race Tasks.txt" for 
 student tasks to be done using this starter pack.
 This program will need changing for sensor start operation
 Data W0 - W8 are used by the Include file
#endrem

symbol Dist = W10
gosub setup			    ; needed at switch-on switches motors off

high LED1			    ; Left hand LED ON
low  LED2,LED3		    ; other LEDs OFF


; loop to await front button, displaying sensor reading
do
   readadc Fsensor,value   ; line sensor value
   
   if value > 140 then
   	high LED3    ; right
	low LED1,LED2
   else
	if  value > 80 then
   	   high LED2    ; middle
	   low LED1,LED3
      else
   	   high LED1   ; left
	   low LED2,LED3
	endif
   endif

   readadc Rsensor,rvalue  ; paper sensor
   if rvalue >10 then      ; if paper detected
	high LED1,LED2,LED3
      gosub wait4paper	   ; wait till paper goes away
      ;goto race           ; start race when paper goes away
   endif

   W9 = timer              ; display timer in W9
   debug			   ; debug takes about 500 ms!
loop  while fswitch = 1    ; loop while front button is not pressed

gosub ronan  ; input from TV remote on long press
race:

;read tuning variables from EEPROM
read CHPLUS   , word speed  ; read speed from EEPROM 
read CHMINUS  , word Kp     ; read Kp from EEPROM 
read VolPLUS  , word Kd     ; read Kd from EEPROM
read VolMINUS , word Dist   ; read distance from EEPROM
;debug
timer = 0                   ; set distance doen track = 0

do
   pause 10
   readadc Fsensor,value   ; line sensor value
   gosub MotorMaths        ; calculate left and right speeds from sensor value
   ;debug
   gosub setmotors         ; check, then set motor speeds
   
   ; check for end of track....
   if timer > Dist then
	   speed = stopspeed
   endif
   ; check for "start again" on back button
  if  bswitch = 0 then myprog
loop