; Include file  V2 for Wham starter pack
; J Fisher 8/2/20
#rem
Updates:
  EEPROM values output on power-up
  TVremote interdigit pause
  setmotors checks speeds
Symbols:
 Inputs
 Outputs
 Variables
 Temp storage
Subroutines:
 -Setup        - sets up PicAxe system on power-up
 -MotorMaths   - calulates leftspeed,rightspeed from Value, and the Tuning
 -checkspeeds  - checks for overflow (>980> and underflow(<0)
 -setmotors
 -ronan        - waits for TV remote
 -readpaper
 -wait4paper   - waits for paper to be removed
#endrem

#picaxe 28x2
#no_table        'No data download - EEPROM not changed

; ======= symbols ==========
'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 TVR    = C.5
symbol LED1 = a.1
symbol LED2 = a.2
symbol LED3 = a.3
symbol Fsensor = 12  ; B.0 symbols must be Channel numbers for readadc
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 


'SYMBOLS FOR VARIABLES
symbol speed      = W1
symbol leftspeed  = W2
symbol rightspeed = W3
symbol value      = W4
symbol oldvalue   = W5
symbol Kd         = W6
symbol Kp         = W7
symbol rvalue     = W8

symbol lastbutton = B55		; (Ronan)
symbol thisbutton = B54		; (Ronan)

;Symbols for EEPROM data locations
symbol CHPLUS   = 0
symbol CHMINUS  = 2
symbol VolPLUS  = 4
symbol VolMINUS = 6
symbol MUTE     = 8

goto myprog

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

    if rightspeed > 32767 then 		; check for underflow
        rightspeed = 0
    endif
    rightspeed = rightspeed max 980  ; limit max positive number
return

setmotors:
    gosub checkspeeds          ; keep speeds in range
    pwmduty lmotor,leftspeed
    pwmduty rmotor,rightspeed
return

setup:
    'Configure ports B and C
    let dirsB = %11111110
    let dirsC = %00000110     '1=output, 0=input
    settimer count 65535	; settimer to count mode Pin C.0
    timer = 0
    ' stop motors
    pwmout pwmdiv4, rmotor, 255,stopspeed   'stop right motor
    pwmout pwmdiv4, lmotor, 255,stopspeed   'stop left motor
 
   ' send message to Terminal window
   sertxd("Drag_race_include_V2.basinc 8/2/20",13,10) 
   
   ; Dump EEPROM values
   pause 100:read CHPLUS, word W0
   sertxd("CH+  = ",#W0,13,10)  
   pause 100:read CHMINUS, word W0
   sertxd("CH-  = ",#W0,13,10)  
   pause 100:read VolPLUS, word W0
   sertxd("Vol+ = ",#W0,13,10)  
   pause 100:read VolMINUS, word W0
   sertxd("Vol- = ",#W0,13,10) 
   pause 100:read MUTE, word W0
   sertxd("Mute = ",#W0,13,10,13,10,13,10)  
   pause 1000

   setfreq em64                'run 8 times faster

return

MotorMaths:
    ;calculates leftspeed, rightspeed from value
    ; and the tuning parameters Kp, Kd, speed 
    W0 = value - oldvalue *Kd
    ; /10
    ; if bit15 = 1 then    ; if W0 is negative
    ;    W0 = -W0 / 10
	;  W0 = -W0
    ;else
    ;    W0 = W0  / 10
    ;endif
;  +value - centre ; error
    W0 = W0 + value - centre

    if bit15 = 1 then    ; if W0 is negative
        W0 = -W0 *Kp / 10
	  W0 = -W0
    else
        W0 = W0 *Kp / 10
    endif
    leftspeed = speed - W0
    rightspeed = speed + W0
    oldvalue = value
return

;====== Ronan ===========


ronan:
  if pinc.5 = 0 then
    return	           	'return if TV receiver powered down
  end if
  high LED1
  W0 = 0  
  do
;  Note fswitch - 0 on entry
    pause 20             ; check for long press
    inc W0               ; count loops
    if  fswitch = 1  then  ; if button released in time
      return              ; then return
    endif
  loop until W0 >100     ; loop till long press detected
  high LED2
  do : loop while   fswitch = 0    ; wait for release
  
  w1 = 0
  do
    irin TVR, thisbutton ' Wham!
    high LED3
    pause 3000           ; extended 5/2/2020
    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 CHPLUS, word w1
	
    case = 17 'CHA-
	  write CHMINUS, word w1
	
    case = 18 'VOL+
	  write VolPLUS, word w1
	
    case = 19 'VOL-
	  write VolMINUS, word w1
	
    case = 20 'MUTE
	  write MUTE, word w1
  
    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
return

readpaper:
    readadc Rsensor,rvalue
return

wait4paper:
   do
      readadc Rsensor,rvalue
   loop while rvalue >7
   return