void PWM2_Handle_ISR(void) {
PLIB_TMR_Period16BitSet(APP_PWM2_TMR_ID, PWM2_Cycle);
PLIB_OC_Buffer16BitSet(APP_PWM2_OC1_ID, PWM2_Start1);
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC1_ID, PWM2_Stop1);
#ifdef APP_PWM2_OC2_ID
PLIB_OC_Buffer16BitSet(APP_PWM2_OC2_ID, PWM2_Start2);
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC2_ID, PWM2_Stop2);
#endif // ifdef APP_PWM2_OC2_ID
}
void APP_Initialize ( void )
{
/* Place the App state machine in its initial state. */
appData.state = APP_STATE_INIT;
appData.InstructionNumber = 0;
irTimer = xTimerCreate("IR Timer", 100/portTICK_PERIOD_MS,pdTRUE, (void*) 1, readIR);
xTimerStart(irTimer, 100);
// these two timers run external interrupts
DRV_TMR1_Start();
DRV_TMR2_Start();
// this timer is used by the external interrupt?
DRV_TMR0_Start();
// these commands set up the PWM for the motors
DRV_OC0_Start();
DRV_OC1_Start();
PLIB_TMR_Period16BitSet(1,1000);
PLIB_OC_PulseWidth16BitSet(0,0);
PLIB_OC_PulseWidth16BitSet(1,0);
PLIB_PORTS_PinDirectionOutputSet (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14);
PLIB_PORTS_PinDirectionOutputSet (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1);
// these lines configure LEDs for operation
PLIB_PORTS_PinDirectionOutputSet (PORTS_ID_0, PORT_CHANNEL_A, PORTS_BIT_POS_3);
PLIB_PORTS_PinSet (PORTS_ID_0, PORT_CHANNEL_A, PORTS_BIT_POS_3);
// this code declares my message queues, declared in the header
MsgQueue_MapEncoder_Interrupt = xQueueCreate( 50, sizeof( StandardMessage ) );
MsgQueue_MapSensor_Interrupt = xQueueCreate( 50, sizeof( StandardMessage ) );
MsgQueue_MapAlgorithm_Instructions = xQueueCreate( 50, sizeof( StandardMessage ) );
// this code declares some GPIO output pins as outputs for me
PLIB_PORTS_DirectionOutputSet (PORTS_ID_0, PORT_CHANNEL_E, 0xFF);
PLIB_PORTS_DirectionOutputSet (PORTS_ID_0, PORT_CHANNEL_B, 0xFF);
PLIB_PORTS_Write(PORTS_ID_0, PORT_CHANNEL_B, 'a');
appData.rightEncoderCount = 0; //SinceLastIntersection
appData.leftEncoderCount = 0; //SinceLastIntersection
appData.leftPath = 0;
appData.rightPath = 0;
appData.forwardPath = 0;
// this code starts tasks for mapper control threads
/*TaskHandle_t Handle_InterpretDataThread;
xTaskCreate((TaskFunction_t) InterpretDataThread,
"InterpretDataThread",
1024, NULL, 1, NULL);
vTaskStartScheduler();*/
}
// *****************************************************************************
// *****************************************************************************
// Section: Instance 0 static driver functions
// *****************************************************************************
// *****************************************************************************
void DRV_OC0_Initialize(void)
{
/* Setup OC0 Instance */
PLIB_OC_ModeSelect(OC_ID_1, OC_COMPARE_PWM_MODE_WITH_FAULT_PROTECTION);
PLIB_OC_BufferSizeSelect(OC_ID_1, OC_BUFFER_SIZE_16BIT);
PLIB_OC_TimerSelect(OC_ID_1, OC_TIMER_16BIT_TMR2);
PLIB_OC_FaultInputSelect(OC_ID_1, OC_FAULT_DISABLE);
PLIB_OC_Buffer16BitSet(OC_ID_1, 0);
PLIB_OC_PulseWidth16BitSet(OC_ID_1, 200);
}
// *****************************************************************************
// *****************************************************************************
// Section: Instance 8 static driver functions
// *****************************************************************************
// *****************************************************************************
void DRV_OC8_Initialize(void)
{
/* Setup OC0 Instance */
PLIB_OC_ModeSelect(OC_ID_9, OC_COMPARE_TURN_OFF_MODE);
PLIB_OC_BufferSizeSelect(OC_ID_9, OC_BUFFER_SIZE_16BIT);
PLIB_OC_TimerSelect(OC_ID_9, OC_TIMER_16BIT_TMR2);
PLIB_OC_FaultInputSelect(OC_ID_9, OC_FAULT_DISABLE);
PLIB_OC_Buffer16BitSet(OC_ID_9, 0);
PLIB_OC_PulseWidth16BitSet(OC_ID_9, 10);
}
int motor_PIDLEncode()
{
float PErrorFix;
float IErrorFix;
int test;
motorData.LDist = (float)motorData.LEncode * MMPERTICK; //mm traveled
motorData.LMes = motorData.LDist * 20.0; //mm/sec
debugU("\nLeft:");
debugUFloat(motorData.LMes);
if(motorData.state != 3)
{
float error = motorData.LSet - motorData.LMes;
motorData.LAvg = ( (error) + (motorData.LAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTOR * error;
IErrorFix = IFACTOR * (motorData.LAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.LOCSet = motorData.LOCSet + test;
}
else
{
if(motorData.RMes == 0) //update RMes if it DNE yet.
{
motorData.RMes = (float)motorData.REncode * MMPERTICK * 20.0;
}
float currRatio = (float)motorData.LMes / (float)motorData.RMes;
float error = (motorData.ratio - currRatio); //*4.0 here orig...
motorData.LAvg = ( (error) + (motorData.LAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTORTURN * error;
IErrorFix = IFACTORTURN * (motorData.LAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.LOCSet = motorData.LOCSet + test;
}
if(motorData.LOCSet < 0)
motorData.LOCSet = 0;
// if(motorData.LOCSet > 600 && motorData.state == 2 )
// motorData.LOCSet = 600;
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
///*
debugUFloat(motorData.LAvg);
debugUFloat(PErrorFix);
debugUFloat(IErrorFix);
debugUInt(motorData.LOCSet);
debugUInt(test);//*/
return motorData.LEncode;
}
void moveRobot(int leftSpeed, int rightSpeed)
{
if(leftSpeed > 0)
{
PLIB_PORTS_PinClear(PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1);
}
else
{
PLIB_PORTS_PinSet(PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1);
}
if(rightSpeed > 0)
{
PLIB_PORTS_PinClear(PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14);
}
else
{
PLIB_PORTS_PinSet(PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14);
}
PLIB_OC_PulseWidth16BitSet(1,leftSpeed);
PLIB_OC_PulseWidth16BitSet(0,rightSpeed);
}
int motor_PIDREncode()
{
float PErrorFix;
float IErrorFix;
int test;
motorData.RDist = (float)motorData.REncode * MMPERTICK;
motorData.RMes = motorData.RDist * 20.0;
debugU("\nRight:");
debugUFloat(motorData.RMes);
if(motorData.state != 2)
{
float error = motorData.RSet - motorData.RMes;
motorData.RAvg = ( (error) + (motorData.RAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTOR * error;
IErrorFix = IFACTOR * (motorData.RAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.ROCSet = motorData.ROCSet + test;
}
else
{
float currRatio = (float)motorData.RMes / (float)motorData.LMes;
float error = 4.0*(motorData.ratio - currRatio);
motorData.RAvg = ( (error) + (motorData.RAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTORTURN * error;
IErrorFix = IFACTORTURN * (motorData.RAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.ROCSet = motorData.ROCSet + test;
}
if(motorData.ROCSet < 0)
motorData.ROCSet = 0;
// if(motorData.ROCSet > 600 && motorData.state == 3 )
// motorData.ROCSet = 600;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
///*
debugUFloat(motorData.RAvg);
debugUFloat(PErrorFix);
debugUFloat(IErrorFix);
debugUInt(motorData.ROCSet);
debugUInt(test);//*/
return motorData.REncode;
}
/* TODO: Add any necessary callback functions.
*/
void TimerInterruptCallback(uintptr_t context, uint32_t alarmCount)
{
static uint32_t index = 0;
Nop();
Nop();
Nop();
Nop();
BSP_LEDToggle(BSP_LED_3);
// update PWM duty cycle with next sample
PLIB_OC_PulseWidth16BitSet(OC_ID_1, sineData[index++]);
// check if we are at the end of the input data array
// if so, reset to beginning of array
if(index == INPUT_DATA_LENGTH)
{
index = 0;
}
}
void PWM2_SetValues(
TMR_PRESCALE pwmPreScale
, int pwmCycle
, int pwmStart1
, int pwmStop1
, int pwmStart2
, int pwmStop2
) {
if ((pwmPreScale == TMR_PRESCALE_VALUE_1) & (pwmCycle < 400)) {
PWM2_doInt = false;
}
if ((pwmPreScale != PWM2_PreScale) | !PWM2_doInt) {
// total update
// Disable Interrupt / Clear Flag
PLIB_INT_SourceDisable(APP_INT_ID, APP_PWM2_TMR_INT_SOURCE);
PLIB_INT_SourceFlagClear(APP_INT_ID, APP_PWM2_TMR_INT_SOURCE);
// Stop the timer
PLIB_TMR_Stop(APP_PWM2_TMR_ID);
// Disable OC's
PLIB_OC_Disable(APP_PWM2_OC1_ID);
#ifdef APP_PWM2_OC2_ID
PLIB_OC_Disable(APP_PWM2_OC2_ID);
#endif // ifdef APP_PWM2_OC2_ID
PWM2_PreScale = pwmPreScale;
PWM2_Cycle = pwmCycle;
PWM2_Start1 = pwmStart1;
PWM2_Stop1 = pwmStop1;
PWM2_Start2 = pwmStart2;
PWM2_Stop2 = pwmStop2;
// Set the prescaler, and set the clock source as internal
PLIB_TMR_PrescaleSelect(APP_PWM2_TMR_ID, pwmPreScale);
// Clear the timer
PLIB_TMR_Counter16BitClear(APP_PWM2_TMR_ID);
// Load the period register
PLIB_TMR_Period16BitSet(APP_PWM2_TMR_ID, pwmCycle);
// OC1 Init
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM2_OC1_ID, pwmStart1);
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC1_ID, pwmStop1);
#ifdef APP_PWM2_OC2_ID
// OC2 Init
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM2_OC2_ID, pwmStart2);
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC2_ID, pwmStop2);
#endif // ifdef APP_PWM2_OC2_ID
// Enable OC 1
PLIB_OC_Enable(APP_PWM2_OC1_ID);
#ifdef APP_PWM2_OC2_ID
// Enable OC 2
PLIB_OC_Enable(APP_PWM2_OC2_ID);
#endif // ifdef APP_PWM2_OC2_ID
if ((PWM2_PreScale == TMR_PRESCALE_VALUE_1) & (PWM2_Cycle < 400)) {
PWM2_doInt = false;
} else {
PWM2_doInt = true;
// Reenable Interrupt
PLIB_INT_SourceEnable(APP_INT_ID, APP_PWM2_TMR_INT_SOURCE);
}
// Start the timer
PLIB_TMR_Start(APP_PWM2_TMR_ID);
} else {
// continuos update
PWM2_Cycle = pwmCycle;
PWM2_Start1 = pwmStart1;
PWM2_Stop1 = pwmStop1;
PWM2_Start2 = pwmStart2;
PWM2_Stop2 = pwmStop2;
}
}
void PWM_Initialize(void) {
// Timer Init
// Stop the timers
PLIB_TMR_Stop(APP_PWM_TMR_ID);
#ifdef APP_PWM2_OC1_ID
PLIB_TMR_Stop(APP_PWM2_TMR_ID);
#endif //ifdef APP_PWM2_OC1_ID
// Disable OC's
PLIB_OC_Disable(APP_PWM_OC1_ID);
#ifdef APP_PWM_OC2_ID
PLIB_OC_Disable(APP_PWM_OC2_ID);
#endif // ifdef APP_PWM_OC2_ID
#ifdef APP_PWM_OC3_ID
PLIB_OC_Disable(APP_PWM_OC3_ID);
#endif // ifdef APP_PWM_OC3_ID
#ifdef APP_PWM_OC4_ID
PLIB_OC_Disable(APP_PWM_OC4_ID);
#endif // ifdef APP_PWM_OC4_ID
#ifdef APP_PWM2_OC1_ID
PLIB_OC_Disable(APP_PWM2_OC1_ID);
#endif // ifdef APP_PWM2_OC1_ID
#ifdef APP_PWM2_OC2_ID
PLIB_OC_Disable(APP_PWM2_OC2_ID);
#endif // ifdef APP_PWM2_OC2_ID
// Set the prescaler, and set the clock source as internal
PLIB_TMR_ClockSourceSelect(APP_PWM_TMR_ID, TMR_CLOCK_SOURCE_PERIPHERAL_CLOCK);
PLIB_TMR_PrescaleSelect(APP_PWM_TMR_ID, APP_PWM_TMR_PRESCALE);
PWM_PreScale = APP_PWM_TMR_PRESCALE;
// Clear the timer
PLIB_TMR_Counter16BitClear(APP_PWM_TMR_ID);
// Load the period register
PLIB_TMR_Period16BitSet(APP_PWM_TMR_ID, APP_PWM_TMR_INIT);
PWM_Cycle = APP_PWM_TMR_INIT;
#ifdef APP_PWM2_OC1_ID
PLIB_TMR_ClockSourceSelect(APP_PWM2_TMR_ID, TMR_CLOCK_SOURCE_PERIPHERAL_CLOCK);
PLIB_TMR_PrescaleSelect(APP_PWM2_TMR_ID, APP_PWM2_TMR_PRESCALE);
PWM2_PreScale = APP_PWM2_TMR_PRESCALE;
// Clear the timer
PLIB_TMR_Counter16BitClear(APP_PWM2_TMR_ID);
// Load the period register
PLIB_TMR_Period16BitSet(APP_PWM2_TMR_ID, APP_PWM2_TMR_INIT);
PWM2_Cycle = APP_PWM2_TMR_INIT;
#endif // ifdef APP_PWM2_OC1_ID
// OC1 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM_OC1_PORTS_ID, APP_PWM_OC1_PORT_CHANNEL, APP_PWM_OC1_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM_OC1_PORTS_ID, APP_PWM_OC1_PORT_CHANNEL, APP_PWM_OC1_PIN);
APP_PWM_OC1_Mode;
PLIB_PORTS_RemapOutput(APP_PWM_OC1_PORTS_ID, APP_PWM_OC1_Function, APP_PWM_OC1_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM_OC1_ID, APP_PWM_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM_OC1_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM_OC1_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM_OC1_ID, APP_PWM_OC1_On);
PWM_Start1 = APP_PWM_OC1_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM_OC1_ID, APP_PWM_OC1_Off);
PWM_Stop1 = APP_PWM_OC1_Off;
#ifdef APP_PWM_OC2_ID
// OC2 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM_OC2_PORTS_ID, APP_PWM_OC2_PORT_CHANNEL, APP_PWM_OC2_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM_OC2_PORTS_ID, APP_PWM_OC2_PORT_CHANNEL, APP_PWM_OC2_PIN);
APP_PWM_OC2_Mode;
PLIB_PORTS_RemapOutput(APP_PWM_OC2_PORTS_ID, APP_PWM_OC2_Function, APP_PWM_OC2_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM_OC2_ID, APP_PWM_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM_OC2_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM_OC2_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM_OC2_ID, APP_PWM_OC2_On);
PWM_Start2 = APP_PWM_OC2_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM_OC2_ID, APP_PWM_OC2_Off);
PWM_Stop2 = APP_PWM_OC2_Off;
#endif // ifdef APP_PWM_OC2_ID
#ifdef APP_PWM_OC3_ID
// OC3 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM_OC3_PORTS_ID, APP_PWM_OC3_PORT_CHANNEL, APP_PWM_OC3_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM_OC3_PORTS_ID, APP_PWM_OC3_PORT_CHANNEL, APP_PWM_OC3_PIN);
APP_PWM_OC3_Mode;
PLIB_PORTS_RemapOutput(APP_PWM_OC3_PORTS_ID, APP_PWM_OC3_Function, APP_PWM_OC3_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM_OC3_ID, APP_PWM_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM_OC3_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM_OC3_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM_OC3_ID, APP_PWM_OC3_On);
PWM_Start3 = APP_PWM_OC3_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM_OC3_ID, APP_PWM_OC3_Off);
PWM_Stop3 = APP_PWM_OC3_Off;
#endif // ifdef APP_PWM_OC3_ID
#ifdef APP_PWM_OC4_ID
// OC4 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM_OC4_PORTS_ID, APP_PWM_OC4_PORT_CHANNEL, APP_PWM_OC4_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM_OC4_PORTS_ID, APP_PWM_OC4_PORT_CHANNEL, APP_PWM_OC4_PIN);
APP_PWM_OC4_Mode;
PLIB_PORTS_RemapOutput(APP_PWM_OC4_PORTS_ID, APP_PWM_OC4_Function, APP_PWM_OC4_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM_OC4_ID, APP_PWM_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM_OC4_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM_OC4_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM_OC4_ID, APP_PWM_OC4_On);
PWM_Start4 = APP_PWM_OC4_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM_OC4_ID, APP_PWM_OC4_Off);
PWM_Stop4 = APP_PWM_OC4_Off;
#endif // ifdef APP_PWM_OC4_ID
#ifdef APP_PWM2_OC1_ID
// PWM2 / OC1 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM2_OC1_PORTS_ID, APP_PWM2_OC1_PORT_CHANNEL, APP_PWM2_OC1_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM2_OC1_PORTS_ID, APP_PWM2_OC1_PORT_CHANNEL, APP_PWM2_OC1_PIN);
APP_PWM2_OC1_Mode;
PLIB_PORTS_RemapOutput(APP_PWM2_OC1_PORTS_ID, APP_PWM2_OC1_Function, APP_PWM2_OC1_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM2_OC1_ID, APP_PWM2_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM2_OC1_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM2_OC1_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM2_OC1_ID, APP_PWM2_OC1_On);
PWM2_Start1 = APP_PWM2_OC1_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC1_ID, APP_PWM2_OC1_Off);
PWM2_Stop1 = APP_PWM2_OC1_Off;
#endif // ifdef APP_PWM2_OC1_ID
#ifdef APP_PWM2_OC2_ID
// PWM2 / OC2 Init
// port inits
PLIB_PORTS_PinClear(APP_PWM2_OC2_PORTS_ID, APP_PWM2_OC2_PORT_CHANNEL, APP_PWM2_OC2_PIN);
PLIB_PORTS_PinDirectionOutputSet(APP_PWM2_OC2_PORTS_ID, APP_PWM2_OC2_PORT_CHANNEL, APP_PWM2_OC2_PIN);
APP_PWM2_OC2_Mode;
PLIB_PORTS_RemapOutput(APP_PWM2_OC2_PORTS_ID, APP_PWM2_OC2_Function, APP_PWM2_OC2_PPSOut);
//Select timer base
PLIB_OC_TimerSelect(APP_PWM2_OC1_ID, APP_PWM2_OC_TMR_BASE);
// Select compare mode
PLIB_OC_ModeSelect(APP_PWM2_OC2_ID, OC_DUAL_COMPARE_CONTINUOUS_PULSE_MODE);
// Set buffer size to 16-bits
PLIB_OC_BufferSizeSelect(APP_PWM2_OC2_ID, OC_BUFFER_SIZE_16BIT);
// Set buffer(primary compare) value
PLIB_OC_Buffer16BitSet(APP_PWM2_OC2_ID, APP_PWM2_OC2_On);
PWM2_Start2 = APP_PWM2_OC2_On;
// Set pulse width(secondary compare) value
PLIB_OC_PulseWidth16BitSet(APP_PWM2_OC2_ID, APP_PWM2_OC2_Off);
PWM2_Stop2 = APP_PWM2_OC2_Off;
#endif // ifdef APP_PWM2_OC2_ID
}
void MOTOR_Tasks ( void )
{
//debugU("RUNNING");
// motor_sendmsg(1, 1000);
// motor_sendmsg(1, 1615);
// motor_sendmsg(3, 1350);
while(1)
{
if(motorData.theQueue != 0)
{
if(xQueuePeek(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY );
motorData.state = motorData.rxMessage.command;
// motorData.state = 1;
// debugUInt(motorData.state);
// debugUInt(motorData.duration);
// communication_sendIntMsg(motorData.rxMessage.command, motorData.rxMessage.duration);
/* Check the application's current state. */
switch ( motorData.state )
{
/* Application's initial state. */
case MOTOR_STATE_INIT: //state 0, do nothing
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
break;
}
case MOTOR_STATE_STRAIGHT: //state 1, go straight.
{
debugU("Straight");
//stop command
if(motorData.rxMessage.int2 == 0)
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_OC1_Stop();
DRV_OC0_Stop();
float time = (float)motorData.n * 0.05;
float Ldistance = MMPERTICK * motorData.LTestTick;
float Rdistance = MMPERTICK * motorData.RTestTick;
motorData.LTestTick = 0;
motorData.RTestTick = 0;
debugUFloat(time);
debugU("Right:\n");
debugUFloat(motorData.RSet);
debugUFloat(Rdistance);
debugU("Left:\n");
debugUFloat(motorData.LSet);
debugUFloat(Ldistance);
debugUFloat(motorData.ratio);
motorData.LSet = 0.0;
motorData.RSet = 0.0;
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
}
else //just go straight
{
motorData.LSet = MAXSPEED;
motorData.RSet = MAXSPEED;
motorData.LOCSet = MAXSPEEDOCSET;
motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Start();
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
}
break;
}
case MOTOR_STATE_TURNRIGHT: //state 2, turn right
{
debugU("Right");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.LSet = angular * (float)outRad;
//motorData.LSet = MAXSPEED;
motorData.LOCSet = (int)(motorData.LSet * 6);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);// - 100);
if(outRad < 11)
{
motorData.RSet = 0.0;
motorData.ROCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
// DRV_OC0_Start();
DRV_OC0_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.RSet = (temp - 10.0) * angular;
// motorData.RSet = ( motorData.ratio * (MAXSPEED));
motorData.ROCSet = (int)(motorData.RSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
DRV_OC0_Start();
}
DRV_OC1_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
case MOTOR_STATE_TURNLEFT: //state 3 turn left.
{
debugU("Left");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.RSet = angular * (float)outRad;
motorData.ROCSet = (int)(motorData.RSet * 6);
// motorData.RSet = MAXSPEED;
// motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
if(outRad < 11)
{
motorData.LSet = 0.0;
motorData.LOCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
// DRV_OC1_Start();
DRV_OC1_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.LSet = (temp - 10.0) * angular;
motorData.LOCSet = (int)(motorData.LSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
DRV_OC1_Start();
}
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
/* The default state should never be executed. */
default:
{
DRV_TMR0_Stop();
DRV_TMR2_Stop();
DRV_TMR0_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
motorData.duration = 0;
/* TODO: Handle error in application's state machine. */
break;
}
} //end switch
}
}
else
{
crash("No Motor MsgQ");
}
}
}
DRV_OC3_SetPulseWidth(int time_value){
PLIB_OC_PulseWidth16BitSet(OC_ID_4, time_value);
}