/****************************************************************************

Module
Locomotion.c

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "LOCOMOTION.h"
#include "PIC32_AD_Lib.h"
#include "../u8g2Headers/common.h"
#include "../ProjectHeaders/portmap.h"
// Hardware
#include <xc.h>
#include <proc/p32mx170f256b.h>
#include <sys/attribs.h> // for ISR macors
#include <stdlib.h>
#include <string.h>
#include <proc/p32mx170f256b.h>
#include <sys/attribs.h> // for ISR macors

// Event & Services Framework

#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_ShortTimer.h"
#include "ES_Port.h"
#include "ES_Timers.h"
#include "EventCheckers.h"

/*----------------------------- Module Defines ----------------------------*/

#define FLOAT_TO_INT(x) ((x)>=0?(uint16_t)((x)+0.5):(uint16_t)((x)-0.5))
//Motor Inputs
#define FWD 0x0F
#define ARC 0x0A
#define TIGHT_ARC 0xAA
#define STOP 0xFF
#define REV 0xBE
#define TIGHT_ARC_R 0xBA
#define TURN_IN_PLACE 0xBB

#define BASE 15 //cm

//Motor Directions
#define FORWARD_DIR 1
#define REVERSE_DIR 2
#define CCW_DIR 3
/*---------------------------- Module Functions ---------------------------*/
void SPI1_LOCOMOTION_Init(void); //SPI1 INitialization
void TIMER2_INIT(void);
void PWM_RIGHT_MOTOR(void);
void PWM_LEFT_MOTOR(void);
void TIMER3_INIT(void);
void IC_INIT_ENCODER_RIGHT(void);
void IC_INIT_ENCODER_LEFT(void);
uint8_t RPM_CALC (uint16_t deltaT);
uint16_t PID_CALC_RIGHT (int16_t RPME);
uint16_t PID_CALC_LEFT (int16_t RPME);
void TIMER4_INIT(void);
void TIMER5_INIT(void);
uint8_t Command_Action (uint8_t Command);
void Motor_Direction(uint8_t Dir);

/*---------------------------- Module Variables ---------------------------*/

// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
// add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
static ES_Event_t DeferralQueue[3 + 1];
//MASTER STATE VARIABLE
static LOCOMOTION_STATE_t Current_State;

/*Left DC Motor variables*/

static uint8_t TargetRPM_LEFT; //RPM set by Potentiometer
volatile static int8_t CurrentRPM_LEFT = 0; //RPM read by encoder
static uint8_t MRPM = 80; //Max RPM per spec sheet of motor
volatile static int8_t RPMError_LEFT; //RPM error for control loop
/*Right DC Motor variables*/
static uint8_t TargetRPM_RIGHT; //RPM set by Potentiometer
volatile static int8_t CurrentRPM_RIGHT = 0; //RPM read by encoder
volatile static int8_t RPMError_RIGHT; //RPM error for control loop
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
* InitFOLLOWER1
Parameters
* My Priority
Returns
* True if no errors
Description
* Initializes System timer 1, SPI1 as slave, sets pin configuration for
* motor controller
****************************************************************************/
bool InitLOCOMOTION(uint8_t Priority)
{
ES_Event_t ThisEvent;

MyPriority = Priority;

// initialize deferral queue for testing Deferal function

ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));

//Initialize System Timer

sysInit();

//Initialize SPI
SPI1_LOCOMOTION_Init();

//Set pins for motor control and encoder

pinMode(RB5, OUTPUT); //LOW RIGHT
digitalWrite(RB5,LOW);
pinMode(RB6,OUTPUT); //HIGH RIGHT
digitalWrite(RB6,HIGH);
pinMode(RB7, OUTPUT);//PWM RIGHT

pinMode(RB8,OUTPUT);//STBY_HIGH
digitalWrite(RB8,HIGH);

pinMode(RB9, OUTPUT);//PWM LEFT

pinMode(RB12,OUTPUT);//HIGH LEFT
 digitalWrite(RB12,HIGH);
pinMode(RB13, OUTPUT);//LOW LEFT
digitalWrite(RB13,LOW);

pinMode(RA3,INPUT); //Encoder RIGHT

pinMode(RB15,INPUT); //ENCODER LEFT

//Set initial State to Idle

Current_State = INIT;

// post the initial transition event

ThisEvent.EventType = ES_INIT;
if (ES_PostToService(MyPriority, ThisEvent) == true)
{
return true;
}
else
{
return false;
}
}

/****************************************************************************
Function
PostFOLLOWER1
Parameters
EF_Event ThisEvent ,the event to post to the queue
Returns
bool false if the Enqueue operation failed, true otherwise
Description
Posts an event to this state machine's queue
****************************************************************************/
bool PostLOCOMOTION(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunFOLLOWER1
Parameters
* ES_Event_t ThisEvent
Returns
* ES_NO_EVENT
Description
* Initializes Pins to use for master's Commands. Replies to master's command
* and print to terminal acknowledgement of those commands
****************************************************************************/
ES_Event_t RunLOCOMOTION(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

//Event deferral recall, must be call before ES_RUN enters anything

ES_RecallEvents(MyPriority, DeferralQueue);

switch(Current_State)
{
//Initialize pins RB6 for LED. RB7 for query
case INIT:
{
if(ThisEvent.EventType == ES_INIT)
{
//Initialization

//Initialize Timers, Input Capture, PWM, and PID COntrol Law

TIMER2_INIT();
PWM_RIGHT_MOTOR();
PWM_LEFT_MOTOR();

TIMER3_INIT();
IC_INIT_ENCODER_RIGHT();
IC_INIT_ENCODER_LEFT();

TIMER4_INIT(); //PID Law Enable

TIMER5_INIT();

//Set Initial Target RPM

TargetRPM_RIGHT = 0;
TargetRPM_LEFT = 0;

Current_State = AWAITING_INSTRUCTIONS;
}
}
break;
//Main State of Service
case AWAITING_INSTRUCTIONS:
{
switch(ThisEvent.EventType)
{
case ES_TIMEOUT:
{
if(ThisEvent.EventParam == ZERO_RPM_RIGHT)
{CurrentRPM_RIGHT = 0;
printf("%d\r\n",CurrentRPM_RIGHT);
}
else if (ThisEvent.EventParam == ZERO_RPM_LEFT)
{CurrentRPM_LEFT = 0;
printf("%d\r\n",CurrentRPM_LEFT);
}
}
break;

case ES_XFER_COMPLETE:
{
switch(ThisEvent.EventParam)
{
case FWD:
{
Motor_Direction(FORWARD_DIR);
TargetRPM_RIGHT= 50;
TargetRPM_LEFT = 50;
}
break;

case ARC:
{
 Motor_Direction(FORWARD_DIR);
TargetRPM_RIGHT= 50;
TargetRPM_LEFT = 48;
}
break;

case TIGHT_ARC:
{
Motor_Direction(FORWARD_DIR);
TargetRPM_RIGHT= 50;
TargetRPM_LEFT = TargetRPM_RIGHT*(2*10-BASE)/(2*10+BASE);
//10 is the turning radius in cm
}
break;

case TIGHT_ARC_R:
{
Motor_Direction(FORWARD_DIR);
TargetRPM_RIGHT= 2;
TargetRPM_LEFT = 50;
//0 is the turning radius in cm
}
break;

case STOP:
{
TargetRPM_RIGHT= 0;
TargetRPM_LEFT = 0;
}
break;

case REV:
{
Motor_Direction(REVERSE_DIR);
TargetRPM_RIGHT= 50;
TargetRPM_LEFT = 50;
}
break;

case TURN_IN_PLACE:
{
Motor_Direction(CCW_DIR);
TargetRPM_RIGHT= 25;
TargetRPM_LEFT = 25;
}
break;

default:{}break;
}
}
break;

default:{}break;
}
}
break;

default:{}break;
}
 return ReturnEvent;
}

/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
SPI1_FOLLOWER1
Parameters
* none
Returns
* none
Description
* Initializes System in slave mode with SS enabled, Enhanced
* buffer mode-8bit. RX interrupts. Uses RA0 as SS. RA1 SDI, RA2 SDO
* RB14 SCK.
****************************************************************************/
void SPI1_LOCOMOTION_Init(void){

static uint32_t Clear_Buffer;

//setting all pins to digital
digiPin();

/*-------------------------------------------------------------------------
* SS1R = RPA0 (0b0000), RA0 is INPUT
* SDI1R = RPA1 (0b0000), RA1 is INPUT
* RPA2R = SDO1 (0b0011), RA2 is OUTPUT
* RPB14R = SCK *ALWAYS, No Mapping Required*, RPB14R = INPUT
-------------------------------------------------------------------------*/
pinMode(RA0, INPUT);
pinMode(RA1, INPUT);
pinMode(RA2, OUTPUT);
pinMode(RB14, INPUT);

//Mapping
SS1R = 0b0000;
SDI1R = 0b0000;
RPA2R = 0b0011;

IEC1CLR = _IEC1_SPI1EIE_MASK; //DIsable SPI1 Fault

IEC1CLR = _IEC1_SPI1RXIE_MASK; //Disable SPI1 RX done
IEC1CLR = _IEC1_SPI1TXIE_MASK; //DIsable SPI1 TX done

//Stopping SPI to modify operations

SPI1CONbits.ON=0;
SPI1CON2=0;

//Read SPI1BUF to clear receive buffer

Clear_Buffer = SPI1BUF;

//Clear Overflow
SPI1STATbits.SPIROV=0;

//SET Baud Rate Generator XFER speed of 100 kHz

SPI1BRG = 0;

//Set Desired Conditions

 SPI1CONbits.MSSEN=0; //Slave Mode Enabled
SPI1CONbits.FRMPOL=0; // pulse is active low
SPI1CONbits.ENHBUF=1; //Enhanced Buffer Mode Enabled
SPI1CONbits.DISSDO=0; //SDO enabled
//Set 8 Bit width XFER rate
SPI1CONbits.MODE16=0;
SPI1CONbits.MODE32=0;
//Set Clock Polarity and CKE
SPI1CONbits.CKE =0;
SPI1CONbits.CKP=1;
SPI1CONbits.SSEN = 1;//SS enabled
SPI1CONbits.MSTEN=0; //Slave Mode ENabled
SPI1CONbits.DISSDI=0;//SDI enabled

/*Setting up Interrupt routine

1- Clear Flags
2- Set up priority
3- Enable if able
--------------------------------*/
SPI1CONbits.STXISEL = 0b00; //Interrupt at end of Transmission
SPI1CONbits.SRXISEL = 0b01; //Interrupt if the RX BUF gets an element

IFS1CLR = _IFS1_SPI1EIF_MASK; //CLear Fault Flag

IFS1CLR = _IFS1_SPI1RXIF_MASK; //CLear Receive Flag
IFS1CLR = _IFS1_SPI1TXIF_MASK; //CLear TX Flag

IPC7bits.SPI1IP = 0b111; //Priority 7

IPC7bits.SPI1IS = 0b11; //Sub-priority 3

IEC1SET = _IEC1_SPI1RXIE_MASK; //Enable SPI1 RX only

//Re-enable SPI functionality

SPI1CONbits.ON=1;
}
/****************************************************************************
ISR Routine
XMIT_INTERRUPT
Parameters
* none
Returns
* none
Description
* Reads the BUF, takes action and replies to the master's commands.
* Sends event to terminal for acknowledgement of command
****************************************************************************/
void __ISR(_SPI_1_VECTOR, IPL7SOFT) XMIT_INTERRUPT(void)
{
if(IFS1bits.SPI1RXIF == 1)
{
uint8_t Command;

Command = SPI1BUF;//read incoming command

if(Command == FWD || Command == ARC ||
Command == TIGHT_ARC || Command == STOP || Command == REV
|| Command == TIGHT_ARC_R || Command == TURN_IN_PLACE)
{
SPI1BUF = Command_Action (Command);//reply to master
}
//Post to write to terminal
 IFS1CLR = _IFS1_SPI1RXIF_MASK; //Clear Flag
}
}
/****************************************************************************
Function
TIMER2_INIT
Parameters

Returns

Description
* Initializes Timer 2 using internal clock, 1:4 pre-scaler, 16 bits.
* Set frequency to 10kHz, Interrupt Disabled. Used for PWMs
****************************************************************************/
void TIMER2_INIT(void)
{
T2CONbits.ON = 0; //Disable timer for properties modification
T2CONbits.TCS = 0; //Select Internal Clock
T2CONbits.TCKPS = 0b010; //Select 1:4 Pre-scaler.
T2CONbits.T32 = 0; //Sets 16 bit timer option
TMR2 = 0; //Clears Timing to begin at 0 ticks
PR2 = 499; //sets PWM period -1 for a frequency of 10 kHz @ 20 MHz PBCLK
IFS0CLR = _IFS0_T2IF_MASK; //Clear Timer2 Flag
IEC0CLR = _IEC0_T2IE_MASK; //Disable local Interrupt
T2CONbits.ON = 1; //Re-enable Timer

/*Timer Enabling and TMR clearing could be build as a function to

test if a certain period of time has elapsed.*/
}
/****************************************************************************
Function
PWM_RIGHT_MOTOR
Parameters

Returns

Description
* Initializes Pulse WIdth Modulation using Timer 2. 16 bit. Interrupt Disabled
* Uses RB7 as output Pin. Default direction is forward
****************************************************************************/
void PWM_RIGHT_MOTOR(void)
{
RPB7R = 0b0101; //Map RB7 to OC1
//Sets motor for CW rotation

OC1CONbits.ON = 0; //disable Output compare and PWM

//Period set on Timer 2
OC1RS = 0; //Sets duty cycle to 0%
OC1R = 0; //Sets initial duty cycle to 0%
OC1CONbits.SIDL = 0; // OPerate in Idle Mode
OC1CONbits.OC32 = 0; // use 16 bit timer source
OC1CONbits.OCTSEL = 0; //Select Timer 2 as source
OC1CONbits.OCM = 0b110; //Select PWM w/o fault protection

OC1CONbits.ON = 1; //Enable Output compare and PWM

}
/****************************************************************************
Function
PWM_LEFT_MOTOR
 Parameters

Returns

Description
* Initializes Pulse WIdth Modulation using Timer 2. 16 bit. Interrupt Disabled
* Uses RB9 as output Pin. Default direction is forward
****************************************************************************/
void PWM_LEFT_MOTOR(void)
{
RPB9R = 0b0101; //Map RB9 to OC3

OC3CONbits.ON = 0; //disable Output compare and PWM

//Period set on Timer 2
OC3RS = 0; //Sets duty cycle to 0%
OC3R = 0; //Sets initial duty cycle to 0%
OC3CONbits.SIDL = 0; // OPerate in Idle Mode
OC3CONbits.OC32 = 0; // use 16 bit timer source
OC3CONbits.OCTSEL = 0; //Select Timer 2 as source
OC3CONbits.OCM = 0b110; //Select PWM w/o fault protection

OC3CONbits.ON = 1; //Enable Output compare and PWM

}
/****************************************************************************
Function
TIMER3_INIT
Parameters

Returns

Description
* Initializes Timer 3 using internal clock, 1:256 pre-scaler, 16 bits.
* Set Period to max to use in tandem with Input Capture. Interrupt Disabled.
* Used for input Capture
****************************************************************************/
void TIMER3_INIT(void)
{
T3CONbits.ON = 0; //Disable timer for properties modification
T3CONbits.TCS = 0; //Select Internal Clock
T3CONbits.TCKPS = 0b111; //Select 1:256 Pre-scaler.
//T3CONbits.T32 = 0; //Does not exist for Timer 3 or 5
TMR3 = 0; //Clears Timing to begin at 0 ticks
PR3 = 0xFFFF; //for input capture math
IFS0CLR = _IFS0_T3IF_MASK; //Clear Timer3 Flag
IEC0CLR = _IEC0_T3IE_MASK; //Disable local Interrupt
T3CONbits.ON = 1; //Re-enable Timer
}
/****************************************************************************
Function
IC_INIT_ENCODER_RIGHT
Parameters

Returns

Description
* Initializes Input Capture 2, 16 bits, using timer 3
* Interrupts on every 4th capture, beginning with rising edge first
* Priority 7. Sets Pin RA3 as Input for Capture
****************************************************************************/
void IC_INIT_ENCODER_RIGHT(void)
{
//Initializing Input Capture #2
IC2R = 0b0000; //Sets pin RA3 as Input Capture Pin

IC2CONbits.ON = 0; //Enable INput Capture

IC2CONbits.FEDGE = 1; //Will capture a Rising Edge as first element
IC2CONbits.C32 = 0; //Establishes that the timer is 16 bits
IC2CONbits.ICTMR = 0; //Sets Timer3 as Timing Source
IC2CONbits.ICI = 0b11; //Interrupts on every 4 Capture Event
IC2CONbits.ICM = 0b110; //Capture rising edge first, and then every edge
IFS0CLR = _IFS0_IC2IF_MASK; //CLear Interrupt Flag
IEC0SET = _IEC0_IC2IE_MASK; //Enable Local Interrupt
IPC2bits.IC2IP = 0b111; //Sets Interrupt Priority to 7
IPC2bits.IC2IS = 0b11; //Sets Sub-priority to 3
IC2CONbits.ON = 1; //Enable INput Capture
}
/****************************************************************************
ISR IC2, Priority 7
DC_MOTOR_ENCODER_RIGHT
Parameters

Returns

Description
* Determines Current RPM by reading Encoder Pulses. It counts the Delta
* Time between 6 pulses. Starts a 150 ms timer, which accounts for the
* motor coming to a stop.
****************************************************************************/
void __ISR(_INPUT_CAPTURE_2_VECTOR, IPL7SOFT) DC_MOTOR_ENCODER_RIGHT(void)
{
//Static Variable Declarations
volatile static uint16_t Edge[4];
static uint8_t Edge_Counter = 0;
static uint8_t Time_Counter = 0;
static uint16_t TimeStart;

//reads buffer elements until buffer is cleared upon receiving an interrupt

while(IC2CONbits.ICBNE !=0){
Edge[Edge_Counter] = (uint16_t)IC2BUF; //Grab time
Edge_Counter++;
};
Time_Counter++;
if (Time_Counter == 1)
{
TimeStart = Edge[0];
}
else if (Time_Counter == 4)
{
Time_Counter = 0; //Reset Counter

//Post event for calibration

CurrentRPM_RIGHT = RPM_CALC(Edge[0]-TimeStart);
}

//Clearing source of the Interrupt

IFS0CLR = _IFS0_IC2IF_MASK;

Edge_Counter = 0;
 ES_Timer_InitTimer(ZERO_RPM_RIGHT, 150); //Timer to Calculate if speed is 0
}
/****************************************************************************
Function
RPM_CALC
Parameters
* Delta Time
Returns
* RPM
Description
* Calculates RPM based on number of pulses read by Input Capture
****************************************************************************/
uint8_t RPM_CALC (uint16_t deltaT)
{
static uint8_t RPM;

RPM=31250/deltaT; //60*6pulses*10^7/(900ppr*130)

return RPM;
}
/****************************************************************************
Function
IC_INIT_ENCODER_LEFT
Parameters

Returns

Description
* Initializes Input Capture 4, 16 bits, using timer 3
* Interrupts on every 4th capture, beginning with rising edge first
* Priority 7. Sets Pin RB15 as Input for Capture
****************************************************************************/
void IC_INIT_ENCODER_LEFT(void)
{
//Initializing Input Capture #4
IC4R = 0b0011; //Sets pin RB15 as Input Capture Pin

IC4CONbits.ON = 0; //Enable INput Capture

IC4CONbits.FEDGE = 1; //Will capture a Rising Edge as first element
IC4CONbits.C32 = 0; //Establishes that the timer is 16 bits
IC4CONbits.ICTMR = 0; //Sets Timer3 as Timing Source
IC4CONbits.ICI = 0b11; //Interrupts on every 4 Capture Event
IC4CONbits.ICM = 0b110; //Capture rising edge first, and then every edge
IFS0CLR = _IFS0_IC4IF_MASK; //CLear Interrupt Flag
IEC0SET = _IEC0_IC4IE_MASK; //Enable Local Interrupt
IPC4bits.IC4IP = 0b111; //Sets Interrupt Priority to 7
IPC4bits.IC4IS = 0b11; //Sets Sub-priority to 3
IC4CONbits.ON = 1; //Enable INput Capture
}
/****************************************************************************
ISR IC4, Priority 7
DC_MOTOR_ENCODER_LEFT
Parameters

Returns

Description
* Determines Current RPM by reading Encoder Pulses. It counts the Delta
* Time between 6 pulses. Starts a 150 ms timer, which accounts for the
 * motor coming to a stop.
****************************************************************************/
void __ISR(_INPUT_CAPTURE_4_VECTOR, IPL7SOFT) DC_MOTOR_ENCODER_LEFT(void)
{
//Static Variable Declarations
volatile static uint16_t Edge[4];
static uint8_t Edge_Counter = 0;
static uint8_t Time_Counter = 0;
static uint16_t TimeStart;

//reads buffer elements until buffer is cleared upon receiving an interrupt

while(IC4CONbits.ICBNE !=0){
Edge[Edge_Counter] = (uint16_t)IC4BUF; //Grab time
Edge_Counter++;
};
Time_Counter++;
if (Time_Counter == 1)
{
TimeStart = Edge[0];
}
else if (Time_Counter == 4)
{
Time_Counter = 0; //Reset Counter
//Post event for calibration
CurrentRPM_LEFT = RPM_CALC(Edge[0]-TimeStart);
}

//Clearing source of the Interrupt

IFS0CLR = _IFS0_IC4IF_MASK;

Edge_Counter = 0;
ES_Timer_InitTimer(ZERO_RPM_LEFT, 153); //Timer to Calculate if speed is 0
}
/****************************************************************************
Function
PID_CALC
Parameters
RPM Error
Returns
Requested Duty Cycle
Description
* Establishes PID Control Law with Integral Anti-Windup
****************************************************************************/
uint16_t PID_CALC_RIGHT (int16_t RPME)
{
//Variable declarations

static int8_t LastError;

static int16_t RequestedDuty;
static PID Gain = {1.2, .2, .5};
static int16_t SumError = 0;

SumError += RPME; //Establishes integral error sum

RequestedDuty = Gain.Proportional*(RPME + (Gain.Integral*SumError)

+Gain.Derivative*(RPME-LastError));
//Set Requested Duty Cycle
if (RequestedDuty >100)
{
 RequestedDuty = 100;
SumError-=RPME; //anti wind up
}
else if (RequestedDuty < 0)
{
RequestedDuty = 0;
SumError-=RPME; //anti wind up
}

LastError = RPME;
return RequestedDuty;
}
/****************************************************************************
Function
PID_CALC
Parameters
RPM Error
Returns
Requested Duty Cycle
Description
* Establishes PID Control Law with Integral Anti-Windup
****************************************************************************/
uint16_t PID_CALC_LEFT (int16_t RPME)
{
//Variable declarations

static int8_t LastError;

static int16_t RequestedDuty;
static PID Gain = {1.2, .2, .5};
static int16_t SumError = 0;

SumError += RPME; //Establishes integral error sum

RequestedDuty = Gain.Proportional*(RPME + (Gain.Integral*SumError)

+Gain.Derivative*(RPME-LastError));
//Set Requested Duty Cycle
if (RequestedDuty >100)
{
RequestedDuty = 100;
SumError-=RPME; //anti wind up
}
else if (RequestedDuty < 0)
{
RequestedDuty = 0;
SumError-=RPME; //anti wind up
}

LastError = RPME;
return RequestedDuty;
}
/****************************************************************************
Function
TIMER4_INIT
Parameters

Returns

Description
* Initializes Timer 4 using internal clock, 1:32 pre-scaler, 16 bits.
 * Set frequency to 200 Hz, Interrupt Enabled, Priority 6.
* Will implement Control Law
****************************************************************************/
void TIMER4_INIT(void)
{
T4CONbits.ON = 0; //Disable timer for properties modification
T4CONbits.TCS = 0; //Select Internal Clock
T4CONbits.TCKPS = 0b101; //Select 1:32 Pre-scaler.
T4CONbits.T32 = 0; //Sets 16 bit timer option
TMR4 = 0; //Clears Timing to begin at 0 ticks
PR4 = 0x3124; //Period -1 for a frequency of 200 Hz @ 20 MHz PBCLK
IFS0CLR = _IFS0_T4IF_MASK; //Clear Timer4 Flag
IEC0SET = _IEC0_T4IE_MASK; //Enable local Interrupt
IPC4bits.T4IP = 0b110; //Sets Interrupt Priority to 6
IPC4bits.T4IS = 0b11; //Sets Sub-priority to 3
T4CONbits.ON = 1; //Re-enable Timer
}
/****************************************************************************
ISR TIMER 4, Priority 6
IPD_ANTI_WINDUP_UPDATE_SPEED
Parameters

Returns

Description
* Implements Control law every 5 ms. Sets OCxRS based on Control Law Output
****************************************************************************/
void __ISR(_TIMER_4_VECTOR, IPL6SOFT) IPD_ANTI_WINDUP_UPDATE_SPEED_RIGHT(void)
{
//Clearing source of the Interrupt
IFS0CLR = _IFS0_T4IF_MASK;

RPMError_RIGHT = TargetRPM_RIGHT - CurrentRPM_RIGHT; //Establishes error

//Set new duty cycle Right Motor

if(TargetRPM_RIGHT < 6)
{
OC1RS = 5.1614*TargetRPM_RIGHT+22.429+
(PID_CALC_RIGHT(RPMError_RIGHT))*499/100;
}
else
{
OC1RS = (PID_CALC_RIGHT(RPMError_RIGHT))*499/100;
}
}
/****************************************************************************
Function
TIMER5_INIT
Parameters

Returns

Description
* Initializes Timer 5 using internal clock, 1:32 pre-scaler, 16 bits.
* Set frequency to 200 Hz, Interrupt Enabled, Priority 6.
* Will implement Control Law
****************************************************************************/
void TIMER5_INIT(void)
{
 T5CONbits.ON = 0; //Disable timer for properties modification
T5CONbits.TCS = 0; //Select Internal Clock
T5CONbits.TCKPS = 0b101; //Select 1:32 Pre-scaler.
//T5CONbits.T32 = 0; //Sets 16 bit timer option
TMR5 = 0; //Clears Timing to begin at 0 ticks
PR5 = 0x3124; //Period -1 for a frequency of 200 Hz @ 20 MHz PBCLK
IFS0CLR = _IFS0_T5IF_MASK; //Clear Timer4 Flag
IEC0SET = _IEC0_T5IE_MASK; //Enable local Interrupt
IPC5bits.T5IP = 0b110; //Sets Interrupt Priority to 6
IPC5bits.T5IS = 0b11; //Sets Sub-priority to 3
T5CONbits.ON = 1; //Re-enable Timer
}
/****************************************************************************
ISR TIMER 5, Priority 6
IPD_ANTI_WINDUP_UPDATE_SPEED
Parameters

Returns

Description
* Implements Control law every 5 ms. Sets OCxRS based on Control Law Output
****************************************************************************/
void __ISR(_TIMER_5_VECTOR, IPL6SOFT) IPD_ANTI_WINDUP_UPDATE_SPEED_LEFT(void)
{
//Clearing source of the Interrupt
IFS0CLR = _IFS0_T5IF_MASK;

RPMError_LEFT = TargetRPM_LEFT - CurrentRPM_LEFT;

//Set new duty cycle Left Motor

if(TargetRPM_LEFT < 6)
{
OC3RS = 5.1614*TargetRPM_LEFT+22.429+
(PID_CALC_LEFT(RPMError_LEFT))*499/100;
}
else
{
OC3RS = (PID_CALC_LEFT(RPMError_LEFT))*499/100;
}
}
/****************************************************************************
Function
* Command_Action
Parameters
* uint8_t Command
Returns
* Master Reply
Description
* Takes action based on master's command and replies
****************************************************************************/
uint8_t Command_Action (uint8_t Command)
{
ES_Event_t INSTRUCTIONS;

INSTRUCTIONS.EventType = ES_XFER_COMPLETE;
INSTRUCTIONS.EventParam = Command;
PostLOCOMOTION(INSTRUCTIONS);

switch (Command)
 {
case FWD:
{
return 0x0F;
}
break;
case ARC:
{
return 0x0A;
}
break;
case TIGHT_ARC:
{
return 0xAA;
}
break;
case TIGHT_ARC_R:
{
return 0xBA;
}
break;
case STOP:
{
return 0xFF;
}
break;
case REV:
{
return 0xBE;
}
break;
case TURN_IN_PLACE:
{
return 0xBB;
}
break;
default:{return 0b0;}break;
}
}
/****************************************************************************
Function
* Motor_Direction
Parameters
* uint8_t Dir
Returns
* none
Description
* set direction of the motors to go FWD, Reverse, CCW
****************************************************************************/
void Motor_Direction(uint8_t Dir)
{
if(Dir == FORWARD_DIR)
{
//Set pins for car to move forward
digitalWrite(RB5,LOW);//LOW RIGHT
digitalWrite(RB6,HIGH);//HIGH RIGHT

digitalWrite(RB12,HIGH);//HIGH LEFT
digitalWrite(RB13,LOW);//LOW LEFT
 }
else if (Dir == REVERSE_DIR)
{
//Set pins for car to move in reverse
digitalWrite(RB5,HIGH);//LOW RIGHT
digitalWrite(RB6,LOW);//HIGH RIGHT

digitalWrite(RB12,LOW);//HIGH LEFT
digitalWrite(RB13,HIGH);//LOW LEFT
}
else if (Dir == CCW_DIR)
{
//Right motor forward
digitalWrite(RB5,LOW);//LOW RIGHT
digitalWrite(RB6,HIGH);//HIGH RIGHT
//Left motor reverse
digitalWrite(RB12,LOW);//HIGH LEFT
digitalWrite(RB13,HIGH);//LOW LEFT
}
}