' SPU_I2C_16_MA3_rev2.bas
' Copyright 2008 BFF Design Ltd
'
' Ian Hopper (BFF Design Ltd) Nov 2008
' http://buggies.builtforfun.co.uk - email Ian@builtforfun.co.uk
'
' For DIY 3 DOF Motion Cockpit Single Chip Signal Processor Unit
'
'=====================================================
' SPU_I2C_16.bas has been provided free of charge WITHOUT ANY WARRANTY
' without even the implied warranty of MERCHANTABILITY or FITNESS FOR
' A PARTICULAR PURPOSE. You assume all responsibility for the consequences
' of your use of SPU_I2C_16.bas.
' 
' SPU_I2C.bas is licensed for your personal NON-COMMERCIAL use only. I am
' happy to negotiate a reasonable charge with you if you wish to use SPU_I2C.bas
' for the purposes of commercial development including but not limited to research
' & development, design feasibility study, prototype development, production and/or
' inclusion within or packaging with domestic or commercial products.
'
' You may not further distribute SPU_I2C_16.bas. 
'====================================================
'
' To read current pot output and send to PC
' Then to receive speed and direction demands back from PC and to write to controllers
' using I2C format
' Serin timed-out to detect loss of data stream from PC
'
' IMPORTANT------------------
' The MD03 motor controllers must be set to the correct I2C bus addresses for the drive
' to function - the addresses are -
'
' Drive 1 (Pitch) - I2C Address 0xB0
' Drive 2 (Roll) - I2C Address 0xB2
' Drive 3 (Heave) - I2C Address 0xB4
'
' See the MD03 user documentation for the mode switch settings on the controller to set
' these addresses.
' ---------------------------
'
' For 28X1 A.3 chip using an external 16 MHz resonator and with with US Digital MA3 magnetic encoder position feedback
' The MA3 encoder should be the 12 bit PWM output version - code will NOT work with either the analgue or 10 bit PWM versions
'
' The encoder pulse outputs should be input to pins 11 through 13 for actuators 1 through 3 respectively.

'Declarations

	symbol	pos1 = w5
	symbol	pos2 = w6
	symbol	pos3 = w7		
	symbol	sdir1 = b4	
	symbol	s1 = b5
	symbol	sdir2 = b6	
	symbol	s2 = b7
	symbol	sdir3 = b8	
	symbol	s3 = b9
	
	symbol	timeout_counter = b20	
	

'Light the Amber LED
	
	switchon 1

'Set the chip frequency to get 9600baud serin to work with the 28X1
	
	setfreq em16
	
'Wait for the start signal from the PC PID controller - the input values are ignored but the program
'will wait here until the PC controller sends a serial packet

	serin 7, N9600_16, ("AB"), b4, b5, b6, b7, b8, b9
	
	pause 4000	'1 sec at 16 MHz
	
'Now setup the I2C comms with the MD03 controllers

	hi2csetup i2cmaster, $B0, i2cslow_16, i2cbyte


	hi2cout [$B0], 3, (0)		'write 0 to the accel register - set max accel
	hi2cout 2, (0)			'write 0 to the speed register - set zero speed
	hi2cout 0, (0)			'write 0 to the command register - set instant stop at present
	
	hi2cout [$B2], 3, (0)		'write 0 to the accel register - set max accel
	hi2cout 2, (0)			'write 0 to the speed register - set zero speed
	hi2cout 0, (0)			'write 0 to the command register - set instant stop at present
	
	hi2cout [$B4], 3, (0)		'write 0 to the accel register - set max accel
	hi2cout 2, (0)			'write 0 to the speed register - set zero speed
	hi2cout 0, (0)			'write 0 to the command register - set instant stop at present
	

	switchoff 1


' Now the main data processing loop - the loop time of the chip sets the loop time of the 
' overall system		
	
MainLoop:

'Get the "12 bit MA3" encoder position values. If encoders are not present then timeout will be 0.164s each missing encoder
'This will cause much slower loop times - comment out any pulsin read not needed....

	pulsin 0, 1, pos1			'measure pos1 as pulsin on pin11
	pulsin 1, 1, pos2			'measure pulsin on pin12
	pulsin 2, 1, pos3			'measure pulsin on pin13
	
'If any pulse is greater than 1720 then set to zero as this probably indicates a timeout, nominal encoder output is 0-1638

	if pos1 > 1720 then
		pos1 = 0
	elseif pos1 > 1638 then
		pos1 = 1638
	end if
	if pos2 > 1720 then
		pos2 = 0
	elseif pos2 > 1638 then
		pos2 = 1638
	end if
	if pos3 > 1720 then
		pos3 = 0
	elseif pos3 > 1638 then
		pos3 = 1638
	end if		
	
'Write the position data to the PC preceeded by "Q" to indicate 0-1638 range

	serout 7, N9600_16, ("Q", b11, b10, b13, b12, b15, b14)
	
'Now read the response from In7 (pin 18) - a short 70ms timeout is set to detect loss of data from the PC

	switchon 2
	
	serin [280,tout], 7, N9600_16, ("AB"), sdir1, s1, sdir2, s2, sdir3, s3
	
'Set the timeout counter back to zero if serial read was successful

	timeout_counter = 0
	
'Check that the direction flags are clean - ie 0, 1 or 2

	if sdir1 > 2 then tout2
	if sdir2 > 2 then tout2
	if sdir3 > 2 then tout2
	
'Send these signals through to the speed controllers on the I2C bus

	hi2cout [$B0], 2, (s1)			'write s1 to the speed register - set s1 speed
	hi2cout 0, (sdir1)			'write sdir1 to the command register - set s1 direction
	
	hi2cout [$B2], 2, (s2)
	hi2cout 0, (sdir2)
	
	hi2cout [$B4], 2, (s3)
	hi2cout 0, (sdir3)
	

'Switch off the green LED

	switchoff 2
	
' Okay - back for another write-read loop
	
	goto MainLoop
	


'Come here if serin times out - ie no data from the PC - stop the drive

tout:

'Index the timeout counter

	timeout_counter = timeout_counter + 1
	
'Only if this is the third short timeout in succession go into the warning routine, otherwise go and try again

	if timeout_counter < 3 then MainLoop

'Important - close down the speed controllers if no data is being received from the PC PID controller.

	hi2cout [$B0], 2, (0)		'write 0 to the speed register - set zero speed
	hi2cout 0, (1)			'write 0 to the command register - set zero speed
	
	hi2cout [$B2], 2, (0)
	hi2cout 0, (1)
	
	hi2cout [$B4], 2, (0)
	hi2cout 0, (1)
	
	pwmduty 1, 0

	
'Flash warning LED on & off

	switchon 0
	
	pause 500
	
	switchoff 0
	
	pause 500
	
	switchon 0
	
	pause 500
	
	switchoff 0
	
'Reset the timeout counter

	timeout_counter = 0
	
'Go back and try another write-read loop
	
	goto MainLoop



'Come here if direction flags look corrupted - stop the drive

tout2:

'Important - close down the speed controllers.

	hi2cout [$B0], 2, (0)		'write 0 to the speed register - set zero speed
	hi2cout 0, (1)			'write 0 to the command register - set zero speed
	
	hi2cout [$B2], 2, (0)
	hi2cout 0, (1)
	
	hi2cout [$B4], 2, (0)
	hi2cout 0, (1)

	
'Flash amber LED on & off to indicate corrupted serin data

	switchon 1
	
	pause 500
	
	switchoff 1
	
	pause 500
	
	switchon 1
	
	pause 500
	
	switchoff 1
	
	pause 2000
	
'Go back and try another write-read loop
	
	goto MainLoop