#include "spimotor.h"

//////////////////////////////////////////////////////////////////////////////
// spimotor5.c
// Laser tweezers motor controller firmware                                 
//
// Description: Firmware for the motorPIC, transfers commands and data
// 			with comPIC over SPI, reads position using quadrature
//			encoders and controls motor voltage using PWM
//			controls Pololu "6-volt" mimi-metal gear motors on XYZ
//			Also controls pinch-clamp motor, either 6 or 12 volts
// Note: Burn PIC with configurarion bit "WATCHDOG TIMER = DISABLED"
// Authors: Steve Smith, Shane Saxon 
//////////////////////////////////////////////////////////////////////////////

#define kSlowDownDistance 0x0400  // avoids overshoot from motor inertia
#define thresholdDuty 150	//minimum PWM dutycycle (range=1024) to keep motor running 
#define thresholdSpeed 2	//minimum targetSpeed to keep motor running

unsigned char cycleCount;
unsigned int timeOutCount;
signed int dutyCycleX, dutyCycleY, dutyCycleZ;
unsigned char temp, readByte, writeByte, transmitIndex;
unsigned char receiveBuffer[5], transmitBuffer[5];
unsigned char limits, blueLED, motorsBusy;
unsigned char controllingXspeed,controllingYspeed,controllingZspeed;
unsigned char accelerationX, accelerationY, accelerationZ;
unsigned char durationX, durationY, durationZ;
unsigned char encoderX[3], encoderY[3], encoderZ[3];
unsigned char directionX, directionY, directionZ;
unsigned char targetSpeedX, targetSpeedY, targetSpeedZ;		
unsigned short long oldPositionX, oldPositionY, oldPositionZ;
unsigned char velocityTimerX,velocityTimerY,velocityTimerZ;
unsigned char positionFeedbackX, positionFeedbackY, positionFeedbackZ;
unsigned short long positionX, positionY, positionZ;
unsigned short long targetX, targetY, targetZ;


////////////////////////////////////////////////////////
void ltInitVariables(void)
{
cycleCount = 0;
transmitIndex = 0;
blueLED = 1; 
limits = 0;
motorsBusy = 0;
timeOutCount = 2;	// cancels false goto on power-up


transmitBuffer[0] = 0;
transmitBuffer[1] = 0;
transmitBuffer[2] = 0;
transmitBuffer[3] = 0;
transmitBuffer[4] = 0;

directionX = 0;
directionY = 0;
directionZ = 0;

controllingXspeed = 0;
controllingYspeed = 0;
controllingZspeed = 0;

positionFeedbackX = 0;
positionFeedbackY = 0;
positionFeedbackZ = 0;

positionX = 0x800000;
positionY = 0x800000;
positionZ = 0x800000;

targetX = 0x800000;
targetY = 0x800000;
targetZ = 0x800000;

velocityTimerX = 1;
velocityTimerY = 2;
velocityTimerZ = 3; // staggered so they don't all go off at once

}

////////////////////////////////////////////////////////
void ltSetupPic(void)
{
//OpenTimer2( TIMER_INT_OFF & T2_PS_1_4 & T2_POST_1_1);	//15kHz PWM (period 155)
OpenTimer2( TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_1);	//5kHz PWM (period 127)

OpenPWM1(127);								//pinch clamp
SetDCPWM1(0);							
OpenPWM3(127);								//X motor
SetDCPWM3(0);
OpenPWM4(127);								//Y motor
SetDCPWM4(0);
OpenPWM5(127);								//Z motor
SetDCPWM5(0);

TRISA = 0x00;								//H-bridge phase pins, sets motor direction
TRISB = 0x00;								//powers Blue LED
TRISD = 0x00;								//encoder control lines, CS, WR, RD, etc..
TRISE = 0x00;								//encoder data port
TRISF = 0xFF;								//motor limit switches and SPI SS input
// PORTG reserved for PWM outputs 2,3,4     
// C2 reserved for output PWM1
PORTD = 0xFF;								//disable all encoder chip select lines 
PORTB = 0xFF;								// start LED on

OpenSPI( SLV_SSOFF, MODE_01, SMPEND);
}

/////////////////////////////////////////////////////////
void ltLoadTransmitBuffer(void)
{
transmitBuffer[0] = transmitIndex;

switch (transmitIndex++)
{
	case kMotorX:
		transmitBuffer[1] = encoderX[0];
		transmitBuffer[2] = encoderX[1];
		transmitBuffer[3] = encoderX[2];
		break;
	case kMotorY:
		transmitBuffer[1] = encoderY[0];
		transmitBuffer[2] = encoderY[1];
		transmitBuffer[3] = encoderY[2];
		break;
	case kMotorZ:
		transmitBuffer[1] = encoderZ[0];
		transmitBuffer[2] = encoderZ[1];
		transmitBuffer[3] = encoderZ[2];
		break;
	case kMotorLimits:
		transmitBuffer[1] = limits;
		transmitBuffer[2] = blueLED;
		transmitBuffer[3] = motorsBusy;
		transmitIndex = kMotorX;		//loops the index
		break;
}

transmitBuffer[4] = cycleCount;
}

//////////////////////////////////////////////////////
void ltReadWriteSPI(void)
{
_asm
	movff writeByte,0xFC9

check0:
   btfss 0xFC7,0,0        //SSPSTAT, BF bit, loops until the byte is transferred
   goto  check0

   movff 0xFC9,readByte      //SSPBUF, received byte from motor copied to ram
_endasm
}

//////////////////////////////////////////////////////
void ltDataTranferSPI(void)
{
ltLoadTransmitBuffer();

if (PORTCbits.RC0 == 0)		//exit if in middle of data transmission
	return;
writeByte = transmitBuffer[0];	//prepare first byte for tranmsit

while (PORTCbits.RC0);
ltReadWriteSPI();
receiveBuffer[0] = readByte;

writeByte = transmitBuffer[1];
ltReadWriteSPI();
receiveBuffer[1] = readByte;

writeByte = transmitBuffer[2];
ltReadWriteSPI();
receiveBuffer[2] = readByte;

writeByte = transmitBuffer[3];
ltReadWriteSPI();
receiveBuffer[3] = readByte;

writeByte = transmitBuffer[4];
ltReadWriteSPI();
receiveBuffer[4] = readByte;
}

////////////////////////////////////////////////////////
void ltWriteEncoders(void)
//enables(low) CS on all encoders and then toggles the write pin
{
PORTDbits.RD5 = 0;		//ENCODER_CS_X for motor X
PORTDbits.RD6 = 0;		//ENCODER_CS_X for motor Y
PORTDbits.RD7 = 0;		//ENCODER_CS_X for motor Z
PORTDbits.RD0 = 0;		//ENCODER_WR
PORTDbits.RD0 = 1;		//done writing
PORTDbits.RD5 = 1;		//disable all encoders
PORTDbits.RD6 = 1;
PORTDbits.RD7 = 1;
}

////////////////////////////////////////////////////////
void ltCenterEncoders(void)  //sets all counters to middle of the range
{
PORTD = ENCODER_CONTROL;
	PORTE = ADDR_RESET;
	ltWriteEncoders();

PORTD = ENCODER_DATA;
	PORTE = 0x00;							//counter lo byte
	ltWriteEncoders();
	PORTE = 0x00;							//middle byte
	ltWriteEncoders();
	PORTE = 0x80;							//hi byte
	ltWriteEncoders();

PORTD = ENCODER_CONTROL;
	PORTE = PRESET_CTR;
	ltWriteEncoders();
}

////////////////////////////////////////////////////////
void ltSetupEncoders(void)
{
PORTD = ENCODER_CONTROL;
	PORTE = MASTER_RESET;
	ltWriteEncoders();
	PORTE = INPUT_SETUP;
	ltWriteEncoders();
	PORTE = QUAD_X4;			//sets encoder resolution
	ltWriteEncoders();
	PORTE = CNTR_RESET;			//zeros the encoders
	ltWriteEncoders();

ltCenterEncoders();
}

////////////////////////////////////////////////////////
void ltReadEncoders(void)
{
TRISE = 0x00;					//configure to write

PORTD = ENCODER_CONTROL;		//prepare all encoders to read positon
	PORTE = LATCH_CNTR;
	ltWriteEncoders();
	PORTE = ADDR_RESET;
	ltWriteEncoders();

TRISE = 0xFF;					//configure to read position

PORTD = ENCODER_DATA;

	PORTDbits.RD5 = 0;			//select x-axis encoder

		PORTDbits.RD3 = 0;		//ENCODER_RD
		PORTDbits.RD3 = 0;
		encoderX[0] = PORTE;
		PORTDbits.RD3 = 1;		// needs double time for setup pulse
		PORTDbits.RD3 = 1;		// needs double time for setup pulse
	
		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderX[1] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderX[2] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

	PORTDbits.RD5 = 1;			//deselect x-axis encoder

	PORTDbits.RD6 = 0;			//select y-axis encoder

		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderY[0] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;
	
		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderY[1] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderY[2] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

	PORTDbits.RD6 = 1;

	PORTDbits.RD7 = 0;			//select z-axis encoder

		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderZ[0] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;
	
		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderZ[1] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

		PORTDbits.RD3 = 0;
		PORTDbits.RD3 = 0;
		encoderZ[2] = PORTE;
		PORTDbits.RD3 = 1;
		PORTDbits.RD3 = 1;

	PORTDbits.RD7 = 1;
}

//////////////////////////////////////////////////////
void ltGetPosition(void)					
{
positionX = encoderX[2];
positionX = positionX << 8;
positionX += encoderX[1];
positionX = positionX << 8;
positionX += encoderX[0];

positionY = encoderY[2];
positionY = positionY << 8;
positionY += encoderY[1];
positionY = positionY << 8;
positionY += encoderY[0];

positionZ = encoderZ[2];
positionZ = positionZ << 8;
positionZ += encoderZ[1];
positionZ = positionZ << 8;
positionZ += encoderZ[0];
}

//////////////////////////////////////////////////////
void ltGoto(void)							
{
if (positionFeedbackX)
	{
		if (directionX)
		{
			if (positionX >= targetX - kSlowDownDistance)
			{
				if (targetSpeedX > thresholdSpeed) 
					{
					targetSpeedX = thresholdSpeed;
					dutyCycleX = thresholdDuty;	//out of 1024 range
					}			
			}
			if (positionX >= targetX)
			{
				targetSpeedX = 0;
				positionFeedbackX = 0; 
				controllingXspeed = 0;
				SetDCPWM3(0);
			}
		}
		else		// moving opposite direction
		{
			if (positionX <= targetX + kSlowDownDistance)
			{
				if (targetSpeedX > thresholdSpeed) 
					{
					targetSpeedX = thresholdSpeed;
					dutyCycleX = thresholdDuty;
					}
			}
			if (positionX <= targetX)
			{
				targetSpeedX = 0;
				positionFeedbackX = 0;
				controllingXspeed = 0;					
				SetDCPWM3(0);
			}
		}	
	}

if (positionFeedbackY)
	{
		if (directionY)
		{
			if (positionY >= targetY - kSlowDownDistance)
			{
				if (targetSpeedY > thresholdSpeed) 
					{
					targetSpeedY = thresholdSpeed;
					dutyCycleY = thresholdDuty;
					}			
			}
			if (positionY >= targetY)
			{
				targetSpeedY = 0;
				positionFeedbackY = 0; 
				controllingYspeed = 0;
				SetDCPWM4(0);
			}
		}
		else
		{
			if (positionY <= targetY + kSlowDownDistance)
			{
				if (targetSpeedY > thresholdSpeed) 
					{
					targetSpeedY = thresholdSpeed;
					dutyCycleY = thresholdDuty;
					}
			}
			if (positionY <= targetY)
			{
				targetSpeedY = 0;
				positionFeedbackY = 0;
				controllingYspeed = 0;					
				SetDCPWM4(0);
			}
		}	
	}
if (positionFeedbackZ)
	{
		if (directionZ)
		{
			if (positionZ >= targetZ - kSlowDownDistance)
			{
				if (targetSpeedZ > thresholdSpeed) 
					{
					targetSpeedZ = thresholdSpeed;
					dutyCycleZ = thresholdDuty;
					}			
			}
			if (positionZ >= targetZ)
			{
				targetSpeedZ = 0;
				positionFeedbackZ = 0; 
				controllingZspeed = 0;
				SetDCPWM5(0);
			}
		}
		else
		{
			if (positionZ <= targetZ + kSlowDownDistance)
			{
				if (targetSpeedZ > thresholdSpeed) 
					{
					targetSpeedZ = thresholdSpeed;
					dutyCycleZ = thresholdDuty;
					}
			}
			if (positionZ <= targetZ)
			{
				targetSpeedZ = 0;
				positionFeedbackZ = 0;
				controllingZspeed = 0;					
				SetDCPWM5(0);
			}
		}	
	}
motorsBusy = positionFeedbackX + 2 * positionFeedbackY + 4 * positionFeedbackZ;
}
//////////////////////////////////////////////////////
void ltSpeedControl(void)			
{
if(controllingXspeed)
{
	velocityTimerX++;
	if (velocityTimerX>5)
		{
		dutyCycleX = dutyCycleX + targetSpeedX;	// increase velocity by amount proportional to targetSpeed
		velocityTimerX = 0;
		}
   if (directionX)
			dutyCycleX = dutyCycleX - 2 * (positionX - oldPositionX);		// decrease velocity by amount proportional to present velocity
	else
			dutyCycleX = dutyCycleX - 2 * (oldPositionX - positionX);

	if (dutyCycleX < 20) 	dutyCycleX = 20;	// making this zero causes encoder`jitter		
	if (dutyCycleX > 350) dutyCycleX = 350;		// 350/1024 makes 9V max from 12V supply for Pololu "6V" motor
	SetDCPWM3(dutyCycleX);
	oldPositionX = positionX;
}	
if(controllingYspeed)
{
	velocityTimerY++;
	if (velocityTimerY>5)
		{
		dutyCycleY = dutyCycleY + targetSpeedY;
		velocityTimerY = 0;
		}
   if (directionY)
			dutyCycleY = dutyCycleY - 2 * (positionY - oldPositionY);
	else
			dutyCycleY = dutyCycleY - 2 * (oldPositionY - positionY);

	if (dutyCycleY < 20) 	dutyCycleY = 20;	// making this zero causes encoder`jitter		
	if (dutyCycleY > 350) dutyCycleY = 350;		// 350/1024 makes 9V max from 12V supply for Pololu "6V" motor
	SetDCPWM4(dutyCycleY);
	oldPositionY = positionY;
}	
if(controllingZspeed)
{
	velocityTimerZ++;
	if (velocityTimerZ>5)
		{
		dutyCycleZ = dutyCycleZ + targetSpeedZ;
		velocityTimerZ = 0;
		}
   if (directionZ)
			dutyCycleZ = dutyCycleZ - 2 * (positionZ - oldPositionZ);
	else
			dutyCycleZ = dutyCycleZ - 2 * (oldPositionZ - positionZ);

	if (dutyCycleZ < 20) 	dutyCycleZ = 20;	// making this zero causes encoder`jitter		
	if (dutyCycleZ > 350) dutyCycleZ = 350;		// 350/1024 makes 9V max from 12V supply for Pololu "6V" motor
	SetDCPWM5(dutyCycleZ);
	oldPositionZ = positionZ;
}		

}

//////////////////////////////////////////////////////
void ltLimits(void)
{
limits = PORTF;

if (limits)
	{
		//add code for turning off motors in blocked direction
	}
}

/////////////////////////////////////////////////////////////////
void ltTimeOut(void)
//turns motors off if timed out, for velocity and voltage commands
{
if (timeOutCount)
	{ 
		timeOutCount--;
		if (timeOutCount == 0)
		{	
			SetDCPWM1(0);
			SetDCPWM3(0);
			SetDCPWM4(0);
			SetDCPWM5(0);
			timeOutCount = 0;
			controllingXspeed = 0;
			controllingYspeed = 0;
			controllingZspeed = 0;
		}
	}
}

///////////////////////////////////////
void main(void)
{
ltInitVariables();
ltSetupPic();
ltSetupEncoders();

while(1)
	{
		ltReadEncoders();
		ltLimits();
		ltDataTranferSPI();

		switch (receiveBuffer[0])
		{	
			case kMotorStop:						//all Stop
				SetDCPWM3(0);
				SetDCPWM4(0);
				SetDCPWM5(0);
				timeOutCount = 0;
				controllingXspeed = 0;
				controllingYspeed = 0;
				controllingZspeed = 0;
				break;

			case kMotorLED:							//LED on/off 
				blueLED = receiveBuffer[1];
				PORTB = blueLED;
				break;

			case kMotorVelocity:					//XYZ velocity
				PORTA = receiveBuffer[1];
				targetSpeedX = receiveBuffer[2];
				targetSpeedY = receiveBuffer[3];
				targetSpeedZ = receiveBuffer[4];
				timeOutCount = 255;
				controllingXspeed = 1;
				controllingYspeed = 1;
				controllingZspeed = 1;
				break;

			case kMotorVoltage:						//XYZ voltage
				PORTA = receiveBuffer[1];			//sets the motor direction
				SetDCPWM3((unsigned int)receiveBuffer[2] * 3);	  //duty cycle max 75% for Pololu "6V" motors
				SetDCPWM4((unsigned int)receiveBuffer[3] * 3);
				SetDCPWM5((unsigned int)receiveBuffer[4] * 3);
				timeOutCount = 5;
				controllingXspeed = 0;
				controllingYspeed = 0;
				controllingZspeed = 0;
				break;

			case kMotorJog:						//XYZ velocity
				PORTA = receiveBuffer[2];			//sets the motor direction
				switch (receiveBuffer[1])		//select motor
				{
					case kMotorX:
						SetDCPWM3((unsigned int)receiveBuffer[3] * 3);	//duty cycle max 75% for Pololu "6V" motors
						break;
					case kMotorY:
						SetDCPWM4((unsigned int)receiveBuffer[3] * 3);	 //duty cycle
						break;
					case kMotorZ:
						SetDCPWM5((unsigned int)receiveBuffer[3] * 3);	 //duty cycle
						break;
					case kMotorPinch:
						SetDCPWM1((unsigned int)receiveBuffer[3] * 3);	 //duty cycle
						break;
				}

				timeOutCount = receiveBuffer[4];	//sets duration
				controllingXspeed = 0;
				controllingYspeed = 0;
				controllingZspeed = 0;
				break;	

			case kMotorGotoX:						//X axis goto position

				targetSpeedX = receiveBuffer[4];				
				targetX = receiveBuffer[3];
				targetX = targetX << 8;
				targetX += receiveBuffer[2];
				targetX = targetX << 8;
				targetX += receiveBuffer[1];
				
				if (positionX < targetX)
				{
					temp = 2;
					directionX = 1;
				}
				else
				{
					temp = 0;
					directionX = 0;
				}
				if (directionY)
					temp += 4;
				if (directionZ)
					temp += 8;
				
				PORTA = temp;  //sets direction bits to H-bridges


				dutyCycleX = 0;
				controllingXspeed = 1;								
				positionFeedbackX = 1;
				break;

			case kMotorGotoY:						//Y axis goto
				targetSpeedY = receiveBuffer[4];				
				targetY = receiveBuffer[3];
				targetY = targetY << 8;
				targetY += receiveBuffer[2];
				targetY = targetY << 8;
				targetY += receiveBuffer[1];
				
				if (positionY < targetY)
				{
					temp = 4;
					directionY = 1;
				}
				else
				{
					temp = 0;
					directionY = 0;
				}
					if (directionX)
						temp += 2;
					if (directionZ)
						temp += 8;

				PORTA = temp;

				dutyCycleY = 0;
				controllingYspeed = 1;									
				positionFeedbackY = 1;
				break;


			case kMotorGotoZ:						//Z axis goto
				targetSpeedZ = receiveBuffer[4];		
				targetZ = receiveBuffer[3];
				targetZ = targetZ << 8;
				targetZ += receiveBuffer[2];
				targetZ = targetZ << 8;
				targetZ += receiveBuffer[1];
		
				if (positionZ < targetZ)
				{
					temp = 8;
					directionZ = 1;
				}
				else
				{
					temp = 0;
					directionZ = 0;
				}
				if (directionX)
					temp += 2;
				if (directionY)
					temp += 4;

				PORTA = temp;

				dutyCycleZ = 0;
				controllingZspeed = 1;									
				positionFeedbackZ = 1;
				break;

			case kMotorCenterEncoders:
				ltInitVariables();
				ltSetupPic();
				ltSetupEncoders();
				break;

			case kMotorSetVelocity:				//XYZ velocity setpoint
				targetSpeedX = receiveBuffer[2];			
				targetSpeedY = receiveBuffer[3];			
				targetSpeedZ = receiveBuffer[4];			
				break;

			case kMotorSetAcceleration:				//XYZ max accelleration
				accelerationX = receiveBuffer[2];
				accelerationY = receiveBuffer[3];
				accelerationZ = receiveBuffer[4];
				break;

			case kMotorSetDuration:				//XYZ max duration
				durationX = receiveBuffer[2];
				durationY = receiveBuffer[3];
				durationZ = receiveBuffer[4];
				break;
		}

	ltTimeOut();

	ltGetPosition();

	ltGoto();

	ltSpeedControl();

	cycleCount++;
	}
}
