;*********************************************************************************
;	gpspeed-demo-0v1	GpSpeed simple demo
;				Terry@RotaryRacer
;*********************************************************************************
;
; This is a basic demo example operating the speed controller.
; It runs the speed controler and displays some data on an LCD display.
; It also measure temperature and displays this.
' For the speed controller it reads the analogue value from the throttle input and
; sets the motors PWM duty cycle as needed.
; It implements soft start and current limiting.
; The PWM frequency is 20 kHz.
;
; Uses fixed point scaled varaibles in 16bit integers for some things. These are scaled by 100.
; So 12.34 Volts is stored as 1234.
;
; Overall operation is:
;
;	Initialise the system
;	MainLoop
;		Read inputs and offset scale as needed
;		Calculate motor speed based on throttle and current
;		Set motor speed via PWM duty cycle
;		Every second toggle LED
;
; Motor speed is set to ramp up or down to the maximum current configured and then
; limited by the throttle position.
;
; The soft start ramp up speed is dependent on the rampSpeed parameter. The core
' main loop of the program runs at 10Hz (100ms). The motors current is ramped up
; by currentDiff / rampSpeed every 100ms. So with ramSpeed set to 50 and starting
; from rest the speed controller will get to about 90% of full power in about 5 seconds.
;
; For: PICAXE-28X2 (PIC18F25K22)
;
; GpSpeed pins are:
;	B.1	LED
;	C.2	Motor PWM
;	C.1	Fan PWM
;	A.0	Analogue Input 0, I0, throttle
;	A.1	Analogue Input 1, I1, misc
;	A.2	Analogue Input 2, Voltage measurement
;	A.3	Analogue Input 3, Current measurement (Hall effect)
;	B.4	Analogue/Digital Input, I2
;
;	B.2	Analogue Input 8, Current measurement (Mosfet)
;	B.0	Error, error from Motor PWM
;	C.3	I2C Clk
;	C.4	I2c Dat
;	C.0	User 0, I2c Int
;	B.3	User 1
;	C.6	SER1 TX
;	C.7	SER1 RX
;
; PWM range at 20kHz: 0 - 400
;

; Options
symbol	useHallThrottle	= 0			; Set to 1 to use Hall effect throttle
symbol	currentMax	= 6000			; Maximum current
symbol	rampSpeed	= 50			; The soft start ramp speed (inverted)

; Symbols for pin names and variables
symbol	pinLed		= B.1			; The LED pin
symbol	pinPwmMotor	= C.2			; The motors PWM pin
symbol	pinPwmFan	= C.1			; The fans PWM pin

symbol	throttle		= w16			; The throttle input value 0 - 1000
symbol	motor		= w17			; The motors power setting 0 - 1000
symbol	voltage		= w18			; The battery voltage scaled by 100
symbol	current		= w19			; The battery currents caled by 100
symbol	value		= w20			; General purpose temporary value
symbol	temperature	= w21			; The temperature reading
symbol	hallThrottle	= b44			; Hall effect throttle being used
symbol	loopCount	= b45			; Loop count

init:
	let adcsetup = %000000000001111	; Initialises ADC inputs
	gosub lcdInit				; Setup I2C hardware for display
	pwmout pinPwmMotor, 99, 0		; Initialises Motor PWM hardware to 20khz. PWM range is 0-400
	pwmout pinPwmFan, 99, 0		; Initialises Fan PWM hardware to 20khz. PWM range is 0-400
	hallThrottle = useHallThrottle
	loopCount = 0
	
start: 	b2=0					;start up writing
	gosub lcdSetPos
	hi2cout 0,("Hello, Welcome" , 255) 
	pause 100
	
	b2=64					
	gosub lcdSetPos
	
	hi2cout 0,("To GP Speed :)" , 255) 
  	pause 2000
  
  	
  	
screenClear:
	let adcsetup = %000000000001111		; Initialises ADC inputs
	gosub lcdInit


				   	                        ; Setup I2C hardware
mainLoop:
	' Read and scale inputs
	readadc10 0,throttle			; Reads the ADC throttle input
	if hallThrottle = 1 then
		if throttle < 210 then		; Limit to Hall Effect's 1 - 4V range
			throttle = 210
		endif
		if throttle > 810 then
			throttle = 810
		endif
		throttle = throttle - 210		; Offset to 0
		throttle = throttle * 10 / 6		; Scale to 0 - 1000
	else
		if throttle < 15 then
			throttle = 0
		endif
		if throttle > 1000 then
			throttle = 1000
		endif
	endif

	readadc10 2,voltage			; Reads the ADC voltage input
	voltage = voltage * 10 / 3			; Scales the voltage so 100 = 1V

	readadc10 3,current			; Reads the ADC current input
	if current > 514 then			; 0 Amps is 512 and current increases with lower values
		current = 514
	endif
	current	 = 514 - current			; Offset current to half way (Sensor provides +- current)
	if current < 0 then
		current = 0;
	endif
	current = current * 119 / 6		; Invert and scale current so 1Amp = 100
		
	; Ramp up/down motor speed to obtain maximum current
	if current < currentMax then
		value = currentMax - current		; Difference between max current and the actual current
		value =10 * value / rampSpeed		; Scale this by the ramp speed
		motor = motor + value			; Update motor speed based on current difference
	else
		value = current - currentMax		; Difference between max current and the actual current
		value = 10 * value / rampSpeed		; Scale this by the ramp speed
		motor = motor - value			; Update motor speed based on current difference
	endif

	; Now limit to range between 0 and throttle position
	if motor < 0 then
		motor = 0;
	endif
	if motor > throttle then
		motor = throttle;			; Always make sure motor speed is less or equal to throttle
	endif
	
	; Set motor speed
	value = motor  * 2 / 5			; Motor speed range is 0 - 1000, PWM range is 0-400
	pwmduty pinPwmMotor, value		; Sets the Motor PWM duty cycle to required speed
	pwmduty pinPwmFan, value		; Sets the Fan PWM duty cycle to required speed

	; Every second processing
	loopCount = loopCount + 1
	if loopCount >= 10 then
		toggle pinLed			; Flash the LED
		gosub display			; Display information on LCD
		loopCount = 0
	endif
	
	; Main loop delay
	pause 100				; 100ms delay
	goto mainLoop

; Display data onto LCD
display:
	b2=0
	gosub lcdSetPos
	
	hi2cout 0,("TH:" , 255) 
	b0 = throttle/4
	b2 = 4
	gosub lcdPutDecimalByte

	b2=73
	gosub lcdSetPos
	
	hi2cout 0,("C:" , 255) 
	w0 = current
	b2 = 74
	gosub lcdPutDecimalWord100
	
	gosub temp
	gosub clock
	return;
	
clock:
	gosub rtcRead					; Read the time from the RTC

	b2 = 64
	gosub lcdSetPos
	
	b0 = b10'					; Print time
	gosub lcdPutDecimal2
	hi2cout 0,(":",255)	
	b0 = b9
	gosub lcdPutDecimal2
	hi2cout 0,(":",255)	
	b0 = b8
	gosub lcdPutDecimal2	
	return
	
temp:
	readadc10 A.1,w8				; Reads the ADC temperature input
;	w8 = w8 * 24					; Scale to degreesC for TMP37
	w8 = w8 * 49					; Scale to degreesC for TMP36

	; Display on LCD if connected
	b2=8
	gosub lcdSetPos
	
	hi2cout 0,("T:" , 255) 
		
	b2 = 10
	w0 = w8
	gosub lcdPutDecimalWord100
	
	return





; Initialises the RTC with a set date/time
rtcInit:
	hi2csetup i2cmaster, $D0, i2cslow, i2cbyte
	hi2cout 0, ($00, $15, $11, $01, $07, $06, $14, $00)
	return

; Reads the current date/time as integers into b13/b12/b11 b10:b9:b8
rtcRead:
	hi2csetup i2cmaster,$D0, i2cslow, i2cbyte
	hi2cin 0, (b8, b9, b10)					;‘ read sec, min, hour
	hi2cin 4, (b11, b12, b13)					;‘ read day, month, year
	
	; Convert BCD values to integers
	b1 = b8 & $0F						; Get the units
	b8 = b8 / 16 * 10 + b1					; Get tens and add units

	b1 = b9 & $0F						; Get the units
	b9 = b9 / 16 * 10 + b1					; Get tens and add units

	b1 = b10 & $0F						; Get the units
	b10 = b10 / 16 * 10 + b1					; Get tens and add units

	b1 = b11 & $0F						; Get the units
	b11 = b11 / 16 * 10 + b1					; Get tens and add units

	b1 = b12 & $0F						; Get the units
	b12 = b12 / 16 * 10 + b1					; Get tens and add units

	b1 = b13 & $0F						; Get the units
	b13 = b13 / 16 * 10 + b1					; Get tens and add units
	
	hi2csetup i2cmaster,$C6,i2cslow,i2cbyte			; Put back to LCD mode
	return

bcdToNum:
	b1 = b0 & $0F						; Get the units
	b0 = b0 / 16 * 10 + b1					; Get tens and add units
	return
	
;******************************************************************************
;	LCD Subroutines
;******************************************************************************

; Initialise the LCD display
lcdInit:
	pause 500
	hi2csetup i2cmaster,$C6,i2cslow,i2cbyte
	hi2cout 0,(254,1,255)
	hi2cout 0,(254,128,255)
	return

; Set the print position to the position defined in b2. Line 1 = +64
lcdSetPos:
	b2 = 128 + b2
	hi2cout 0,(254,b2,255)
	return

; Print a single chracher that is in b0 at the position defined in b2. Line 1 = +64
lcdPutChar:
	b2 = 128 + b2
	hi2cout 0,(254,b2,b0,255)
	pause 10
	return

;Print the value in b0 as a decimal at the position defined in b2. Line 1 = +64
lcdPutDecimalByte:
	b2 = 128 + b2
	b3 = b0 / 100 + 48
	b0 = b0 // 100;
	b4 = b0 / 10 + 48
	b0 = b0 // 10;
	b5 = b0 + 48
	
	hi2cout 0,(254,b2,b3,b4,b5,255)
	pause 10
	return

;Print the value in w0 as a decimal at the position defined in b2. Line 1 = +64
lcdPutDecimalWord:
	b2 = 128 + b2
	b3 = w0 / 10000 + 48
	w0 = w0 // 10000;
	b4 = w0 / 1000 + 48
	w0 = w0 // 1000;
	b5 = w0 / 100 + 48
	w0 = w0 // 100;
	b6 = w0 / 10 + 48
	w0 = w0 // 10;
	b7 = w0 + 48

	if b3 = "0" then
		b3 = " "
		if b4 = "0" then
			b4 = " "
			if b5 = "0" then
				b5 = " "
				if b6 = "0" then
					b6 = " "
				endif
			endif
		endif
	endif
		
	writei2c 0,(254,b2,b3,b4,b5,b6,b7,255)
;	pause 10

	return

;Print the value in w0 as a decimal scaled by 100 at the position defined in b2. Line 1 = +64
lcdPutDecimalWord100:
	b2 = 128 + b2
	b3 = w0 / 10000 + 48
	w0 = w0 // 10000;
	b4 = w0 / 1000 + 48
	w0 = w0 // 1000;
	b5 = w0 / 100 + 48
	w0 = w0 // 100;
	b6 = w0 / 10 + 48
	w0 = w0 // 10;
	b7 = w0 + 48

	if b3 = "0" then
		b3 = " "
		if b4 = "0" then
			b4 = " "
		endif
	endif
		
	writei2c 0,(254,b2,b3,b4,b5,".",b6,b7,255)
	pause 10
	return

;Print the value in b0 as a decimal. Only two digits at current position
lcdPutDecimal2:
	b4 = b0 / 10 + 48
	b0 = b0 // 10;
	b5 = b0 + 48
	
	hi2cout 0,(b4,b5,255)
	pause 10
	return