;*********************************************************************************
;	gpspeed-rr10-0v1	RR10's GpSpeed program
;				Terry@RotaryRacer, 2019-03-03
;*********************************************************************************
;
; This is the simple RR10 GpSpeed running code. It expects the GpSpeedDataLog add on
; board and GpSdisplay boards to be present and connected.
;
; DataLog
;=========================================================
; It uses the pixaxe download/debug serial port to communicate with the user
; so the user must enable the terminal debug window in the Picaxe programming
; software to use this program. Note on a picaxe-28X2 the baud rate will be 9600
;
; It first waits for 4 seconds for an <Enter key> to be pressed. If this happens
; the system will enter command mode allowing the user to type commands. Otherwise
; the system goes into run mode logging the data.
; It then sets up the real time clock to a fixed date and time.
; It then measures and stores some data to the data log, once per second.
;
; The command mode can be extended to support, for example, setting the RTC.
;
;
; Note, when the picaxe serial cable is used as a serial debug port for inputing typed data
; from the user as it is in this program, then the picaxe cannot be programmed once the program
; has started. To program it in this case: Start the programming and then press the
; GpSpeed reset button. There is a small window of opportunity when the picaxe starts up were
; it can be programmed in this case.
;
; Uses fixed point scaled variables in 16bit integers for some things. These are scaled by 100.
; So 12.34 Volts is stored as 1234.
;
; Motor and display
;=========================================================
; 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.
;
; 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
;
;		Process any swiych inputs
;
;		Every second
;			Read RTC date/time
;			Display driver info on LCD
;			Set driver LED's
;			Send data on telemetry
;		Every 10 seconds
;			Store data in log
;
; 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
;
; I2C devices
;=========================================================
; $7C	LCD Display
; $10	GpDisplay LED's and Switches
; $D0	RTC
; $A0	EEPROM memory
; $A2	EEPROM memory
; $A4	EEPROM memory
; $A6	EEPROM memory
;

; Compile time options
; Uncomment to use Hall effect throttle
;#define	UseHallThrottle
; Uncomment to set the date/time to fixed values
#define	DoSetTime

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

; Settings
symbol	currentMax	= 6000			; Maximum current
symbol	rampSpeed	= 50			; The soft start ramp speed (inverted)

; Symbols to allocate variables. Variables w0-w7 (b0 - b15) are used as temporary variables
symbol	year		= b16			; The year
symbol	month		= b17			; The month
symbol	day		= b18			; The day
symbol	hour		= b19			; The hour
symbol	minute		= b20			; The minute
symbol	second		= b21			; The second
symbol	throttle	= w11			; The throttle input value 0 - 1000
symbol	throttle.lsb	= b22
symbol	throttle.msb	= b23
symbol	voltage		= w12			; The battery voltage scaled by 100
symbol	voltage.lsb	= b24
symbol	voltage.msb	= b25
symbol	current		= w13			; The battery currents scaled by 100
symbol	current.msb	= b26
symbol	current.lsb	= b27
symbol	speed		= w14			; The battery currents scaled by 100
symbol	temperature	= b30			; The temperature reading
symbol	spare1		= b31			; The temperature reading
symbol	logPos		= w16			; The current datalog write position
symbol	pos		= w17			; Position
symbol	tmp		= w18			; General purpose temporary value
symbol	motor		= w19			; The motors power setting 0 - 1000
symbol	value		= w20			; General purpose temporary value

symbol	loopCount1	= b50			; Loop count seconds
symbol	loopCount10	= b51			; Loop count 10 seconds
symbol	leds		= b52			; The LED settings (bits)
symbol	switches	= b53			; The Switch settings (bits)

; Symbols for CPU's internal EEPROM
symbol	eepromLogPos	= 0			; The current position in the dataLog

init:
;	setfreq m4				; Sets CPU operational clock rate
	let adcsetup = %000000000001111		; Initialises ADC inputs
	gosub lcdInit				; Setup the display
	gosub logInit				; Setup the data log
	gosub rtcInit				; Initialise the real time clock
	hsersetup B9600_8,0			; Setup serial port for telemetry at 9600 baud

	#ifdef DoSetTime
		; Setup the date/time to a fixed starting value
		sertxd ("Set RTC's date and time", 13, 10)
		year = 19
		month = 1
		day = 1
		hour = 0
		minute = 0
		second = 0
		gosub rtcWrite
	#endif

	loopCount1 = 0
	loopCount10 = 0
	
start:
	b2=0					;start up writing
	gosub lcdSetPos
	hi2cout $7C,($40, "RR10 Started 0v1") 
	
	gosub commandCheck			; Check if any user commands

mainLoop:
	; The main processing loop. Once per second
	;sertxd ("Main loop", 13, 10)
	
	readadc10 0,throttle			; Reads the ADC throttle input
	#ifdef UseHallThrottle
		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

	; Read the switches
	hi2cin [$10],1,(switches)
	
	; Process switches
	; TBD ...

	; Every second processing
	loopCount1 = loopCount1 + 1
	if loopCount1 >= 10 then
		toggle pinLed				; Flash the GpSpeed LED
		gosub rtcRead				; Read the date and time

		readadc10 A.1,temperature		; Reads the ADC temperature input
		; temperature = temperature * 24	; Scale to degreesC for TMP37
		temperature = temperature * 49		; Scale to degreesC for TMP36

		hi2cout [$10],0,(leds);			; Set the LED's
		gosub display				; Display information on LCD

		; Telemetry output		
		hserout 0, (year, month, day, hour, minute, second, throttle.lsb, throttle.msb, voltage.lsb, voltage.msb, current.lsb, current.msb)

		loopCount1 = 0
	endif


	; Every 10 seconds processing
	loopCount10 = loopCount10 + 1
	if loopCount10 >= 100 then
		; Add to the datalog
		gosub logAppend
		;sertxd ("LogAppended: ", #logPos, 13, 10)
		loopCount10 = 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:
	; Display time on LCD
	b2 = 64
	gosub lcdSetPos
	
	b0 = hour					; Print time
	gosub lcdPutDecimal2
	hi2cout $7C,(":")	
	b0 = minute
	gosub lcdPutDecimal2
	hi2cout $7C,(":")	
	b0 = second
	gosub lcdPutDecimal2	
	return
	
temp:
	; Display temeparture on LCD
	b2=8
	gosub lcdSetPos
	
	hi2cout $7C,("T:") 
		
	b2 = 10
	w0 = temperature
	gosub lcdPutDecimalWord100
	
	return


; Command input
commandCheck:
	; Waits for 4 seconds for any user response. If any enters command mode
	sertxd ("Type <Enter key> to go into command mode", 13, 10)
	serrxd [4000], b0

	if b0 = 13 then
		sertxd ("GpSpeed Commands", 13, 10)
		sertxd (" q   - Quit to run mode", 13, 10)
		sertxd (" d   - Download data log", 13, 10)
		sertxd (" r   - Reset data log to start", 13, 10)
		do
			; Command loop
			sertxd ("cmd> ")
			serrxd b0
			sertxd (b0, 13, 10)
			if b0 = "d" then
				gosub commandDownloadLog
			elseif b0 = "r" then
				gosub logReset
			endif
		loop while b0 != "q"
	endif
	reconnect
	return

commandDownloadLog:
	; Downloads the data log to the serial debug lines
	sertxd ("GpSpeed data log: ", #logPos, " entries", 13, 10)
	if logPos = 0 then
		return
	endif
	
	w2 = logpos - 1
	for pos = 0 to w2
		w0 = pos
		gosub logGet
		sertxd (#day, "/", #month, "/", #year, " ", #hour, ":", #minute, ":", #second)
		sertxd (" ", #throttle, " ", #voltage, " ", #current)
		sertxd (13,10)
	next
	sertxd ("end", 13,10)
	return

	
;******************************************************************************
;	RTC Subroutines
;******************************************************************************
;
; Provides functions to read and write the date and time from the RTC.
; Note the RTC uses BCD (binary coded decimal) for its data rather than plain binary integers
;
rtcInit:
	; Initialises the RTC with a set date/time
	hi2csetup i2cmaster, $D0, i2cslow, i2cbyte
	return

rtcSetTime:
	; Hard coded date/time in BCD: sec, min, hour, wday, day, month, year, control: 15/12/01 12:00:00
	hi2csetup i2cmaster, $D0, i2cslow, i2cbyte
	hi2cout 0, ($00, $00, $12, $01, $01, $12, $15, $00)
	return

rtcRead:
	; Reads the current date/time as integers into year,month,day,hour,minute,second
	hi2csetup i2cmaster, $D0, i2cslow, i2cbyte
	hi2cin 0, (b2, b3, b4)					; read sec, min, hour
	hi2cin 4, (b5, b6, b7)					; read day, month, year
	
	; Convert BCD values to integers
	b1 = b2 & $0F						; Get the units
	second = b2 / 16 * 10 + b1					; Get tens and add units

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

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

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

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

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

rtcWrite:
	; Writes the current date/time as integers from year,month,day,hour,minute,second to the RTC
	b1 = second // 10						; Convert second to BCD
	b2 = second / 10 * 16 + b1

	b1 = minute // 10						; Convert minute to BCD
	b3 = minute / 10 * 16 + b1

	b1 = hour // 10						; Convert hour to BCD
	b4 = hour / 10 * 16 + b1

	b1 = day // 10						; Convert day to BCD
	b5 = day / 10 * 16 + b1

	b1 = month // 10						; Convert month to BCD
	b6 = month / 10 * 16 + b1

	b1 = year // 10						; Convert year to BCD
	b7 = year / 10 * 16 + b1

	hi2csetup i2cmaster, $D0, i2cslow, i2cbyte
	hi2cout 0, (b2, b3, b4)					; write sec, min, hour
	hi2cout 4, (b5, b6, b7)				; write day, month, year
	return

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

rtcTest:
	; Simple test loop reading the date and time and outputing it to the serial debug lines
	gosub rtcRead
	sertxd (#day, "/", #month, "/", #year, " ", #hour, ":", #minute, ":", #second, 13, 10)
	pause 1000
	goto rtcTest
	

;******************************************************************************
;	Log Subroutines
;******************************************************************************
;
; The data logs write position is stored in the CPU's internal EEPROM area as a
; 16 bit pointer to a complete 16 byte data record.
; Note that the CPU's internal EEPROM is reset to 0 when a new program is downloaded
;
logInit:
	; Initialise I2C bus, access to I2C EEPROM chips and read old logPos
	hi2csetup i2cmaster,$A0, i2cslow, i2cword
	read eepromLogPos, b0, b1
	logPos = b1 << 8 | b0
	return
	
logReset:
	; Reset data logging position to the start, 0
	logPos = 0
	b0 = logPos & $FF
	b1 = logPos >> 8
	write eepromLogPos, b0, b1
	return
	
logAppend:
	; Append a data log entry and increment the log position
	b0 = logPos >> 11 & $FE + $A0
	w1 = logPos << 4
	hi2csetup i2cmaster, b0, i2cslow, i2cword
	hi2cout w1, (year, month, day, hour, minute, second, throttle.lsb, throttle.msb, voltage.lsb, voltage.msb, current.lsb, current.msb)
	pause 5				; It takes up to 5ms for the EEPROM chip to write the data

	logPos = logPos + 1
	b0 = logPos & $FF
	b1 = logPos >> 8
	write eepromLogPos, b0, b1
	return

logGet:
	; Fetch a log entry from the location given in w0
	b2 = w0 >> 11 & $FE + $A0
	w0 = w0 << 4
	hi2csetup i2cmaster, b2, i2cslow, i2cword
	hi2cin w0, (year, month, day, hour, minute, second, throttle.lsb, throttle.msb, voltage.lsb, voltage.msb, current.lsb, current.msb)
	return


;******************************************************************************
;	LCD Subroutines
;******************************************************************************

; Initialise the LCD display
lcdInit:
	; Reset the LCD	
	;low pinLcdReset
	;pause 10
	;high pinLcdReset
	;pause 100

;	hi2csetup i2cmaster,$3E,i2cslow,i2cbyte		; Setup I2C hardware with address of LCD nodule
	hi2csetup i2cmaster,$7C,i2cslow,i2cbyte		; Setup I2C hardware with address of LCD nodule

	hi2cout ($00, $38)	; Set function
	hi2cout ($00, $39)	; Set function
	hi2cout ($00, $14)	; Set Oscilator frequency
	hi2cout ($00, $79)	; Set Contrast
	hi2cout ($00, $50)	; Set PowerOn/Contrast
	
	hi2cout ($00, $6C)	; Follower control
	hi2cout ($00, $0C)	; Set display on/off
	
	pause 100
	gosub lcdClear		; Clear display
	return

; Set the print position to the position defined in b2. Line 1 = +64
lcdClear:
	hi2cout [$7C],($00, $01)
	return
	
; Set the print position to the position defined in b2. Line 1 = +64
lcdSetPos:
	b2 = 128 + b2
	hi2cout [$7C],($00, b2)
	return

; Print a single character that is in b0 at the position defined in b2. Line 1 = +64
lcdPutChar:
	b2 = 128 + b2
	hi2cout [$7C],($80,b2,$40,b0)
	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 [$7C],($80,b2,$40,b3,b4,b5)
	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 = $30 then
		b3 = $20
		if b4 = $30 then
			b4 = $20
			if b5 = $30 then
				b5 = $20
				if b6 = $30 then
					b6 = $20
				endif
			endif
		endif
	endif

	hi2cout	[$7C],($80,b2,$40,b3,b4,b5,b6,b7)
;	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 = $30 then
		b3 = $20
		if b4 = $30 then
			b4 = $20
		endif
	endif
		
	hi2cout [$7C],($80,b2,$40,b3,b4,b5,$2E,b6,b7)
	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 [$7C],($40,b4,b5)
	pause 10
	return