'StarterStarter Pack FOR WHAM 2 
'=========================================================
' 26/1/2020
'PORTS
'a.0 = Back pushbutton
'a.1 = Left LED
'a.2 = middle LED
'a.3 = right LED 3
'
'b.0 = Front sensor
'
'c.0 = Odometry input
'c.1 = Left motor PWM output
'c.2 = Right motor PWM output
'c.3 = Front pushbutton 
'c.4 = No connection
'c.5 = TV remote receiver
'c.6 = Right sensor
'c.7 = Left sensor
'
'DIRECTIVES
#picaxe 28X2
#no_table        'No data download
'
'SYMBOLS FOR INPUTS/OUTPUTS 
symbol bswitch = pina.0  'back switch
symbol fswitch = pinc.3  'front switch
symbol rmotor = c.2
symbol lmotor = c.1
symbol LED1 = a.1
symbol LED2 = a.2
symbol LED3 = a.3
symbol TVR = C.5

; symbols must use Channel numbers for readadc
symbol Fsensor = 12  ; B.0 
symbol Lsensor = 19  ; C.7
symbol Rsensor = 18  ; C.6

'SYMBOLS FOR Useful numbers
symbol stopspeed = 490   ; PWM output to stop the motors
symbol centre = 128      ; Line sensor centre reading 
symbol Kp =10      ; amount of Derivative. 

'DECLARE SYMBOLS FOR VARIABLES
symbol leftspeed  = W1
symbol rightspeed = W2
symbol value      = W3
symbol oldvalue   = W4
symbol correction = W5
symbol corrected  = W6		; Sensor reading after Derivative applied


'************************************************************************
'                        INITIALISATION
'************************************************************************
    gosub setup



; loop to await front button, displaying sensor
do
   readadc Fsensor,value   ; line sensor value
   
   if value > centre then
   	high LED2    ; middle
   else
   	low LED2
   endif
   debug			; debug takes about 500 ms!
loop  while fswitch = 1    ; loop while button not pressed

gosub ronan
low LED1,LED2,LED3

; loop controlling motor speeds from sensor
top:

do
   oldvalue = value
   readadc Fsensor,value        ; line sensor
   correction = value - oldvalue * Kp    ; extra to add because of rate of change
   corrected = value + correction

   ; check for underflow 
   if corrected > 32000 then
      corrected = 0
   endif
; *******************Your program****************************
; use "corrected" as your sensor reading,
; use "centre" as the line sensor reading for straight
; use "rightspeed" and "leftspeed" as your motorspeeds

	; this just stops the motors...
	
   ;rightspeed = stopspeed 
   ;leftspeed  = stopspeed 
   
   read  0,word leftspeed
   read  2,word rightspeed
   
   
; ***********************************************
   gosub checkspeeds  ; check speed values
   gosub setmotors       ; output speeds to motors

loop


;====================SUBROUTINES===============
checkspeeds:
if leftspeed > 32767 then 		; check for underflow
  leftspeed = 0
endif
leftspeed = leftspeed max 1023  ; limit max positive number

if rightspeed > 32767 then 		; check for underflow
  rightspeed = 0
endif
rightspeed = rightspeed max 1023  ; limit max positive number
return
;================
setmotors:
    pwmduty lmotor,leftspeed
    pwmduty rmotor,rightspeed
return
;================
setup:
    setfreq em64                'run faster
    'Configure ports B and C
    let dirsB = %11111110
    let dirsC = %00000110     '1=output, 0=input

    ' stop motors
    pwmout pwmdiv4, rmotor, 255,stopspeed   'stop right motor
    pwmout pwmdiv4, lmotor, 255,stopspeed   'stop left motor

   sertxd("Wham_starter_pack 2.bas 26/1/2020",13,10)  ' send message to Terminal window
return
;================
'************************************************************************

symbol lastbutton = B55
symbol thisbutton = B54

ronan:
  if pinc.5=0 then
    return	           	'return if TV receiver powered down
  end if
  high LED1

; fswitch - 0 on entry
  pause 2000     ; check for long press
  if  fswitch = 1  then  ; if button released in time
     return              ; then return
  endif
  high LED2
  w1 = 0
do
  irin TVR, thisbutton ' Wham!
  high LED3
  pause 1000
  low LED3
  
  select thisbutton
  case <= 9 
      if lastbutton > 9 then
        w1 = 0
      endif
      if thisbutton = 9 then
        thisbutton=0
      else
	  thisbutton = thisbutton+1 'on simulation, remove this line. on remote, include this line.
	endif
	w1 = w1 * 10  + thisbutton 'multiplies by 10 first

  case = 21 'if power button is pressed
	return   ; from ronan

  case = 16 'CHA+
	write 0,word w1
	read 0, word w2
	
  case = 17 'CHA-
	write 2,word w1
	read 2, word w3
	
  case = 18 'VOL+
	write 4,word w1
	read 4, word w4
	
  case = 19 'VOL-
	write 6,word w1
	read 6, word w5
	
  case = 20 'MUTE
	write 8,word w1
	read 8, word w6
  
   
  case = 101 '  OK , reads all values and debugs.
	read 0, word w0
	read 2, word w1
	read 4, word w2
	read 6, word w3
	read 8, word w4
	debug
	thisbutton = 101
  endselect
  
  lastbutton = thisbutton
loop



; end of program