;*********************************************************************************
;	gpspeed-run_0v1	GpSpeed running version
;				Terry@RotaryRacer
;*********************************************************************************
;
; This is a basic example operating 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	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
	pwmout pinPwmMotor, 99, 0		; Initialises Motor PWM hardware to 20khz. PWM range is 0-400
	hallThrottle = useHallThrottle
	loopCount = 0

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

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