//
//	Version 1.0
//		Using 16F88
//
//	Version 1.1
//		tried to fix when the motor start at full speed
//
//	Version 1.2
//		Increased the res of the steering
//		Attempted to slow the inner wheel during turning
//


#include <16F88.h>

#fuses INTRC_IO,NOPROTECT,NOWDT,NOPUT,NOMCLR,NOBROWNOUT,NOLVP,NODEBUG

#device ADC=8	// Set ADC to 8bit
#device *=16

#use delay(clock=8000000)

#use RS232(Baud=9600,Xmit=PIN_B5,Rcv=PIN_B2,brgh1ok,ERRORS,Stream=COMMS)

#define hi(x)	(*(&x+1))
#define lo(x)	(*(&x))

#byte	TMR0 = 0x01
#bit	MathCarry = 0x03.0

//For Changing Edge Select H-L and L-H Directly
// instead of through ext_int_edge(l_to_h)
#bit EdgeDirection = 129.6

#byte RCSTA = 0x18
#byte	TXREG = 0x19
#byte RCREG = 0x1A
#byte TXIF = 12.4
#byte	RCIF = 12.5
#bit OERR = 24.1
#bit FERR = 24.2
#bit TrisB_RS232_TX = 134.5	// pin_B5 input/output
#bit TXEN = 152.5					// Transmit enable
#bit TRMT = 152.1					// Transmit empty

#byte	Option_Reg = 0x81

#byte SSPBUF =  0x13
#byte SSPcon = 0x14
#byte CCP1CON = 0x17
#byte SSPstat = 0x94

#bit DA_BIT = SSPSTAT.5
#bit RW_BIT = SSPBUF.0

#define MtrLeftR1_On		output_high(PIN_B0)	// Pin 6
#define MtrLeftR1_Off	output_low(PIN_B0)
#define MtrLeftT1_On		output_high(PIN_B1)	// Pin 7
#define MtrLeftT1_Off	output_low(PIN_B1)
//#define RS232 RX									// Pin 8
#define MtrRightT4_On	output_high(PIN_B3)	// Pin 9
#define MtrRightT4_Off	output_low(PIN_B3)
#define MtrRightT3_On	output_high(PIN_B4)	// Pin 10
#define MtrRightT3_Off	output_low(PIN_B4)
//#define RS232 TX									// Pin 11
#define MtrLeftT2_On		output_high(PIN_B6)	// Pin 12
#define MtrLeftT2_Off	output_low(PIN_B6)
#define MtrRightR2_On	output_high(PIN_B7)	// Pin 13
#define MtrRightR2_Off	output_low(PIN_B7)

#define SteerLeft_On			output_high(PIN_A6)	// Pin 15
#define SteerLeft_Off		output_low(PIN_A6)
#define SteerRight_On		output_high(PIN_A7)	// Pin 16
#define SteerRight_Off		output_low(PIN_A7)

#define MtrLeftRelay_On   MtrLeftR1_On;
#define MtrLeftRelay_Off  MtrLeftR1_Off;
#define MtrLeftTransL_On  MtrLeftT1_On;
#define MtrLeftTransL_Off MtrLeftT1_Off;
#define MtrLeftTransR_On  MtrLeftT2_On;
#define MtrLeftTransR_Off MtrLeftT2_Off;

#define MtrRightRelay_On   MtrRightR2_On;
#define MtrRightRelay_Off  MtrRightR2_Off;
#define MtrRightTransL_On  MtrRightT3_On;
#define MtrRightTransL_Off MtrRightT3_Off;
#define MtrRightTransR_On  MtrRightT4_On;
#define MtrRightTransR_Off MtrRightT4_Off;

struct adc_control {
	short	ADON;
	short	unused;
	short	Go;
	int	Channel : 3;
	short	ADCS0;
	short ADCS1;
}		ADCON0;
#byte	ADCON0 = 0x1F

struct adc_control1 {
	int	unused : 3;
	int	VCFG : 2;
	short	ADCS2;
	short ADFM;
}		ADCON1;
#byte	ADCON1 = 0x9F

#byte	ADRESH = 0x1E
#byte	ADRESL = 0x9E
#byte	ANSEL = 0x9B

#byte PORTB = 0x06

#define ON 1
#define OFF 0

//#define LeftForward  0b00000010
//#define LeftReverse  0b00001001
//#define RightForward 0b00010000
//#define RightReverse 0b11000000
#define Forward 1
#define Reverse 0

short	bLeftTurn=0;
short	bRightTurn=0;
long	lHeartbeat = 0;
int	i=0;
int	iADC_Step=0;
int	iADC_Channel=0;
int	iAnalogIN[4];
int	iDelay=0;
int	iAVG=0;
long	iMotorPeriod = 140;
long	lMtr_Steer_Pos_AVG=0;
int	iMtr_Steer_Pos_RAW=0;
long	lMtr_Steer_Trg_AVG=0;
int	iMtr_Steer_Trg_RAW=0;
signed int iMtr_Steer_Target=0;
signed int iMtr_Steer_Pos=0;
signed int iMtr_Speed=0;
signed int iMtr_speed_CompL=0;
signed int iMtr_speed_CompR=0;
int	iTimer_Step[4] = {0,0,0,0};

typedef struct {
	short	bToggle;
	short	DirFWD;
	short	DirREV;
	short	Enable;
	int	iStep;
	int	iDuration;
	int	iSpd;
	int	iDelay;
	signed int	iSpeed;
} mtr_ctrl;

mtr_ctrl	Mtr_Left;
mtr_ctrl	Mtr_Right;
mtr_ctrl Mtr_Steer;

//#include "apa102.c"

//**********************************************
//Handles timer1 interrupt
//**********************************************
#int_timer1
void Handle_t1(void)
{
	set_timer1(0);
	lHeartBeat++;
	if(Mtr_Left.iDelay) Mtr_Left.iDelay--;
	if(Mtr_Right.iDelay) Mtr_Right.iDelay--;
}



/*
void ReadEEprom(void)
{
	iThisMotorNum = read_eeprom(10);
	iAnalogThreshold = read_eeprom(11);
	hi(lCurrPos) = read_eeprom(12);
	lo(lCurrPos) = read_eeprom(13);
	hi(lFullTravDist) = read_eeprom(14);
	lo(lFullTravDist) = read_eeprom(15);
	hi(lMtrStallPRE) = read_eeprom(16);
	lo(lMtrStallPRE) = read_eeprom(17);
}


void WriteEEprom(void)
{
	write_eeprom(10,iThisMotorNum);
	write_eeprom(11,iAnalogThreshold);
	write_eeprom(12,hi(lCurrPos));
	write_eeprom(13,lo(lCurrPos));
	write_eeprom(14,hi(lFullTravDist));
	write_eeprom(15,lo(lFullTravDist));
	write_eeprom(16,hi(lMtrStallPRE));
	write_eeprom(17,lo(lMtrStallPRE));
	SaveEEprom = OFF;
}
*/


//**********************************************
// Read analog input
//
//**********************************************
void ReadAnalog(void)
{
	// Step logic used to read the analog input
	switch(iADC_Step)
	{
		case 0:
		{
			iADC_Channel++;			// Setup analog channel
			if(iADC_Channel>3) iADC_Channel=0;
			//ADCON0.Channel = iADC_Channel;
			set_adc_channel(iADC_Channel);
			iADC_Step++;
		}	break;

		case 1:
		{
			iADC_Step++;				// Wait the required acquisition time (one scan)
		}	break;

		case 2:
		{
			//ADCON0.Go = 1;				// Start the next AD converstion
			iADC_Step++;
		}	break;

		case 3:
		{
			if(ADCON0.Go==0)			// Wait for converstion to finish
			{
				iAnalogIN[iADC_Channel] = read_adc(); // ADRESH;
				iADC_Step=0;			// Start it all again
			}
		}	break;

		default:
		{
			iADC_Step=0;
		}	break;
	}

}


int test(mtr_Ctrl* motor) {
	motor->iSpd = 4;
}


void main()
{
	disable_interrupts(INT_TIMER0);
	disable_interrupts(INT_TIMER1);
	disable_interrupts(INT_TIMER2);
	disable_interrupts(INT_CCP1);
	disable_interrupts(INT_EXT);
	disable_interrupts(int_AD);
	disable_interrupts(int_rda);
	disable_interrupts(global);

	set_tris_a(0b00111111);
	set_tris_b(0b00000000);
	port_b_pullups(FALSE);

   MtrLeftR1_Off;
   MtrLeftT1_Off;
   MtrLeftT2_Off;
	Mtr_Left.Enable=0;

   MtrRightR2_Off;
   MtrRightT3_Off;
   MtrRightT4_Off;
	Mtr_Right.Enable=0;

	SteerLeft_Off;
	bLeftTurn=0;
	SteerRight_Off;
	bRightTurn=0;

	setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2|RTCC_8_BIT);
	setup_timer_1(T1_INTERNAL|T1_DIV_BY_2);
	setup_timer_2(T2_DISABLED,1,1);
	//CCP1CON = 0;
//	SSPcon = 0b000100010;

	//Option_Reg = 0b11010000;

	setup_adc(ADC_CLOCK_INTERNAL);
	setup_adc_ports(sAN0|sAN1|sAN2|sAN3);
	ADCON1.ADFM = 1;

	//setup_spi(SPI_MASTER | SPI_L_TO_H | SPI_CLK_DIV_64);

	enable_interrupts(int_timer1);
	//enable_interrupts(int_EXT);
	//enable_interrupts(INT_SSP);
	enable_interrupts(global);


	Mtr_Left.iSpeed = 0;
	Mtr_Right.iSpeed = 0;

	// Wait for LittleBits controller to be running first
	fprintf(COMMS,"Waiting for LittleBits%c%c",11,13);
	lHeartbeat=0;
	while(lHeartbeat < 20)
	{
		ReadAnalog();

		if((iAnalogIN[2] < 120) || (iAnalogIN[2] > 140)) lHeartbeat=0;
	}

	fprintf(COMMS,"Running...%c%c",11,13);

	for(;;)
	{
		ReadAnalog();

		// Cant allow move request to go below 8 due to 136 offset
		if(iAnalogIN[2] <= 9) iAnalogIN[2] = 9;
		// Note: not quite half way, centre point is 136 instead of 128
		iMtr_Speed = 136- (signed int)iAnalogIN[2];
		// Adjust speed to compensate for steering
		if(iMtr_Steer_Pos < 0){
			iMtr_Speed_CompL = ABS(iMtr_Steer_Pos);
		}
		else{
			iMtr_Speed_CompL = 1;
		}
		if(iMtr_Steer_Pos > 0){
			iMtr_Speed_CompR = ABS(iMtr_Steer_Pos);
		}
		else{
			iMtr_Speed_CompR = 1;
		}

// Left motor
		Mtr_Left.iSpeed = iMtr_Speed / iMtr_speed_CompL;

		// Left Motor Drive forward
		if(Mtr_Left.iSpeed>9){
			Mtr_Left.DirFWD = 1;
			Mtr_Left.DirREV = 0;
			Mtr_Left.iSpd = (int)Mtr_Left.iSpeed;
		}
		// Left Motor Drive reverse
		if(Mtr_Left.iSpeed<-9){
			Mtr_Left.DirFWD = 0;
			Mtr_Left.DirREV = 1;
			Mtr_Left.iSpd = (int)(-1 * Mtr_Left.iSpeed);
		}
		// Left Motor Drive during steering
		if((Mtr_Left.iSpeed<=9) && (Mtr_Left.iSpeed>=-9)){
			// Monitor steering, if turning then run motor
			Mtr_Left.DirFWD = 0;
			Mtr_Left.DirREV = 0;
			//if(bLeftTurn) { Mtr_Left.DirFWD = 1; Mtr_Left.DirREV = 0; }
			//if(bRightTurn){ Mtr_Left.DirFWD = 0; Mtr_Left.DirREV = 1; }
			//Mtr_Left.iSpd = (int)iMtr_Speed_Comp;
		}

		// Step logic used to handle Left motor
		switch(Mtr_Left.iStep)
		{
			case 0:
			{
				// Forward command
				if(Mtr_Left.DirFWD){
					Mtr_Left.iStep = 1;
				}
				if(Mtr_Left.DirREV){
					Mtr_Left.iStep = 15;
				}
   	   	MtrLeftTransL_Off;
   	      MtrLeftTransR_Off;
				Mtr_Left.iDelay = 5;
				Mtr_Left.Enable = 0;
			}	break;

			case 1:
			{
				// Wait for delay to finish
				if(Mtr_Left.iDelay == 0){
					// Turn off reverse relay
					MtrLeftRelay_Off;
					Mtr_Left.iDelay = 5;
					Mtr_Left.iStep++;
				}
			}	break;

			case 2:
			{
				// Wait for delay to finish
				if(Mtr_Left.iDelay == 0){
					Mtr_Left.iStep++;
				}
			}	break;

			case 3:
			{
				// Running forward, until change
				if(!Mtr_Left.DirFWD){
					Mtr_Left.iStep=0;
				}
				Mtr_Left.Enable = 1;
			}	break;

			case 15:
			{
				// Wait for delay to finish
				if(Mtr_Left.iDelay == 0){
					// Turn off reverse relay
					MtrLeftRelay_On;
					Mtr_Left.iDelay = 10;
					Mtr_Left.iStep++;
				}
			}	break;

			case 16:
			{
				// Wait for delay to finish
				if(Mtr_Left.iDelay == 0){
					Mtr_Left.iStep++;
				}
			}	break;

			case 17:
			{
				// Running reverse, until change
				if(!Mtr_Left.DirREV){
					Mtr_Left.iStep=0;
				}
				Mtr_Left.Enable = 1;
			}	break;

			default:
			{
				Mtr_Left.iStep=0;
			}	break;

		}
		// Used to control LEFT motor, PWM style
      if(Mtr_Left.bToggle && Mtr_Left.Enable)
      {
			if(Mtr_Left.DirFWD){
   	      MtrLeftTransL_Off;
	         MtrLeftTransR_On;
			}
			if(Mtr_Left.DirREV){
   	      MtrLeftTransL_On;
	         MtrLeftTransR_Off;
			}
         if(Mtr_Left.iDuration) Mtr_Left.iDuration--;
         else
         {
            Mtr_Left.iDuration = iMotorPeriod - Mtr_Left.iSpd;
            Mtr_Left.bToggle = 0;
         }
      }
      else
      {
         MtrLeftTransL_Off;
         MtrLeftTransR_Off;
         if(Mtr_Left.iDuration) Mtr_Left.iDuration--;
         else
         {
            Mtr_Left.iDuration = Mtr_Left.iSpd;
            Mtr_Left.bToggle = 1;
         }
      }


// Right motor
		// Note: not quite half way, centre point is 136 instead of 128
		Mtr_Right.iSpeed = iMtr_Speed / iMtr_speed_CompR;

		// Right Motor Drive forward
		if(Mtr_Right.iSpeed>9){
			Mtr_Right.DirFWD = 1;
			Mtr_Right.DirREV = 0;
			Mtr_Right.iSpd = (int)Mtr_Right.iSpeed;
		}
		// Right Motor Drive reverse
		if(Mtr_Right.iSpeed<-9){
			Mtr_Right.DirFWD = 0;
			Mtr_Right.DirREV = 1;
			Mtr_Right.iSpd = (int)(-1 * Mtr_Right.iSpeed);
		}
		// Right Motor Drive during steering
		if((Mtr_Right.iSpeed<=9) && (Mtr_Right.iSpeed>=-9)){
			// Monitor steering, if turning then run motor
			Mtr_Right.DirFWD = 0;
			Mtr_Right.DirREV = 0;
			//if(bRightTurn) { Mtr_Right.DirFWD = 1; Mtr_Right.DirREV = 0; }
			//if(bRightTurn){ Mtr_Right.DirFWD = 0; Mtr_Right.DirREV = 1; }
			//Mtr_Right.iSpd = (int)iMtr_Speed_Comp;
		}

		// Step logic used to handle Right motor
		switch(Mtr_Right.iStep)
		{
			case 0:
			{
				// Forward command
				if(Mtr_Right.DirFWD){
					Mtr_Right.iStep = 1;
				}
				if(Mtr_Right.DirREV){
					Mtr_Right.iStep = 15;
				}
   	   	MtrRightTransL_Off;
   	      MtrRightTransR_Off;
				Mtr_Right.iDelay = 5;
				Mtr_Right.Enable = 0;
			}	break;

			case 1:
			{
				// Wait for delay to finish
				if(Mtr_Right.iDelay == 0){
					// Turn off reverse relay
					MtrRightRelay_Off;
					Mtr_Right.iDelay = 5;
					Mtr_Right.iStep++;
				}
			}	break;

			case 2:
			{
				// Wait for delay to finish
				if(Mtr_Right.iDelay == 0){
					Mtr_Right.iStep++;
				}
			}	break;

			case 3:
			{
				// Running forward, until change
				if(!Mtr_Right.DirFWD){
					Mtr_Right.iStep=0;
				}
				Mtr_Right.Enable = 1;
			}	break;

			case 15:
			{
				// Wait for delay to finish
				if(Mtr_Right.iDelay == 0){
					// Turn off reverse relay
					MtrRightRelay_On;
					Mtr_Right.iDelay = 10;
					Mtr_Right.iStep++;
				}
			}	break;

			case 16:
			{
				// Wait for delay to finish
				if(Mtr_Right.iDelay == 0){
					Mtr_Right.iStep++;
				}
			}	break;

			case 17:
			{
				// Running reverse, until change
				if(!Mtr_Right.DirREV){
					Mtr_Right.iStep=0;
				}
				Mtr_Right.Enable = 1;
			}	break;

			default:
			{
				Mtr_Right.iStep=0;
			}	break;

		}
		// Used to control Right motor, PWM style
      if(Mtr_Right.bToggle && Mtr_Right.Enable)
      {
			if(Mtr_Right.DirFWD){
   	      MtrRightTransL_Off;
	         MtrRightTransR_On;
			}
			if(Mtr_Right.DirREV){
   	      MtrRightTransL_On;
	         MtrRightTransR_Off;
			}
         if(Mtr_Right.iDuration) Mtr_Right.iDuration--;
         else
         {
            Mtr_Right.iDuration = iMotorPeriod - Mtr_Right.iSpd;
            Mtr_Right.bToggle = 0;
         }
      }
      else
      {
         MtrRightTransL_Off;
         MtrRightTransR_Off;
         if(Mtr_Right.iDuration) Mtr_Right.iDuration--;
         else
         {
            Mtr_Right.iDuration = Mtr_Right.iSpd;
            Mtr_Right.bToggle = 1;
         }
      }

// Steering actuator

      lMtr_Steer_Pos_AVG += (long)iAnalogIN[0];
      lMtr_Steer_Trg_AVG += (long)iAnalogIN[3];
      iAVG++;
      if(iAVG>99){
         iMtr_Steer_Pos_RAW = (int)(lMtr_Steer_Pos_AVG / 100);
         lMtr_Steer_Pos_AVG = 0;
         iMtr_Steer_Trg_RAW = (int)(lMtr_Steer_Trg_AVG / 100);
         lMtr_Steer_Trg_AVG = 0;
         iAVG = 0;
      }

      // Transfer analog into signed int to give direction
      iMtr_Steer_Pos = (128 - (signed int)iMtr_Steer_Pos_RAW);
      iMtr_Steer_Target = (128 - (signed int)iMtr_Steer_Trg_RAW);

      if(iMtr_Steer_Target == iMtr_Steer_Pos){
         SteerLeft_Off;
         bLeftTurn=0;
         SteerRight_Off;
         bRightTurn=0;
      }

      lHeartbeat=0;
      if(iMtr_Steer_Target > iMtr_Steer_Pos + 1){
         bLeftTurn=1;
         SteerLeft_On;
         SteerRight_Off;
      }
      if(iMtr_Steer_Target < iMtr_Steer_Pos - 1){
         bRightTurn=1;
         SteerLeft_Off;
         SteerRight_On;
      }
    }
}