void MOTOR_Initialize ( void )
{
/* Place the App state machine in its initial state. */
motorData.state = MOTOR_STATE_INIT;
/* Place the App state machine in its initial state. */
motorData.RxSeqNum = 0;
motorData.TxSeqNum = 0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.REncodeTotal = 0;
motorData.LEncodeTotal = 0;
motorData.LDist = 0;
motorData.RDist = 0;
motorData.LSet = 0;
motorData.RSet = 0;
motorData.LMes = 0;
motorData.RMes = 0;
motorData.LAvg = 0.0;
motorData.RAvg = 0;
motorData.LOCSet = 0;
motorData.ROCSet = 0;
motorData.ratio = 0;
motorData.RTestTick = 0;
motorData.LTestTick = 0;
motorData.n = 0;
//motorData.rxMessage.command = 0;
//motorData.rxMessage.duration = 0;
motorData.theQueue = xQueueCreate(MOTORQUEUELENGTH, sizeof(MOTOR_MESSAGE)); //sizeof(appData.rxMessage));
if(motorData.theQueue == 0)
{
crash("E:Create Motor msgQ");
//failed to create queue
}
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_G, 0x0002);
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_D, 0x0003);
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_C, 0x4000);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_D, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_D, PORTS_BIT_POS_0, 0);
DRV_OC1_Initialize();
DRV_OC0_Initialize();
//DRV_OC1_Enable();
//DRV_OC0_Enable(); //same thing as start
DRV_OC1_Stop();
DRV_OC0_Stop();
//25.6 us per count
//count to 200 = 51.2ms per rollover
DRV_TMR0_Initialize(); //OC Timer
DRV_TMR1_Initialize(); //IC Timer
DRV_TMR2_Initialize(); //duration Timer
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_TMR0_Stop(); //pwm timer
DRV_TMR1_Stop(); //encoder timer 5ms
DRV_TMR2_Stop(); //duration timer 1ms
// theTimerInit(52);
}
void BSP_LEDStateSet(BSP_LED led, BSP_LED_STATE state)
{
/* Update the state of the LED */
if (led == LED_1 || led == LED_2)
{
PLIB_PORTS_PinWrite(PORTS_ID_0, PORT_CHANNEL_A, led, state);
}
else
{
PLIB_PORTS_PinWrite(PORTS_ID_0, PORT_CHANNEL_D, led, state);
}
}
void BSP_LEDOn (BSP_LED led)
{
/* Switch ON the LED */
if (led == LED_1 || led == LED_2)
{
PLIB_PORTS_PinWrite(PORTS_ID_0, PORT_CHANNEL_A, led, 1);
}
else
{
PLIB_PORTS_PinWrite(PORTS_ID_0 , PORT_CHANNEL_D , led, 1);
}
}
void BSP_LEDOff(BSP_LED led)
{
/* switch OFF the LED */
if (led == LED_1 || led == LED_2)
{
PLIB_PORTS_PinWrite(PORTS_ID_0, PORT_CHANNEL_A, led, 0);
}
else
{
PLIB_PORTS_PinWrite(PORTS_ID_0 , PORT_CHANNEL_D , led, 0);
}
}
void SYS_PORTS_PinWrite ( PORTS_MODULE_ID index,
PORTS_CHANNEL channel,
PORTS_BIT_POS bitPos,
bool value )
{
PLIB_PORTS_PinWrite ( index, channel, bitPos, value );
}
void MOTOR_Initialize ( void )
{
/* Place the App state machine in its initial state. */
motorData.state = MOTOR_STATE_INIT;
/* Place the App state machine in its initial state. */
motorData.RxSeqNum = 0;
motorData.TxSeqNum = 0;
//motorData.rxMessage.command = 0;
//motorData.rxMessage.duration = 0;
motorData.theQueue = xQueueCreate(MOTORQUEUELENGTH, sizeof(MOTOR_MESSAGE)); //sizeof(appData.rxMessage));
if(motorData.theQueue == 0)
{
crash("E:Create Motor msgQ");
//failed to create queue
}
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_G, 0x0002);
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_D, 0x0003);
PLIB_PORTS_DirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_C, 0x4000);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_D, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_D, PORTS_BIT_POS_0, 0);
// _OC1_Initialize();
// DRV_OC0_Initialize();
// //DRV_OC1_Enable();
// //DRV_OC0_Enable(); //same thing as start
// DRV_ODRVC1_Stop();
// DRV_OC0_Stop();
// //25.6 us per count
// //count to 200 = 51.2ms per rollover
// DRV_TMR0_Initialize(); //OC Timer
// DRV_TMR1_Initialize(); //IC Timer
// DRV_TMR0_CounterClear();
// DRV_TMR1_CounterClear();
// DRV_TMR0_Start();
// DRV_TMR1_Stop();
initDebugU();
theTimerInit(52);
}
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");
}
}
}
void BSP_LEDStateSet(BSP_LED_PORT led_port, BSP_LED_CHANNEL led_channel, BSP_LED_STATE led_state)
{
PLIB_PORTS_PinWrite( PORTS_ID_0, led_channel, led_port, led_state );
}
void MOTOR_Tasks ( void )
{
while(1)
{
if(motorData.theQueue != 0)
{
if(xQueuePeek(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY );
/* int i = 0;
while( motorData.rxMessage.SeqNum != motorData.RxSeqNum)
{
i++;
if(i == MOTORQUEUELENGTH)
{
motorData.RxSeqNum++; //drop a message of that seq number by incrementing
i = 0;
}
xQueueSendToBack(motorData.theQueue, (void*)&(motorData.rxMessage), 0); //send to back
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), 0 ); //get another
}//*/
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_TMR1_Stop();
// DRV_TMR1_CounterClear();
// DRV_OC1_Stop();
// DRV_OC0_Stop();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_STRAIGHT: //state 1, go straight.
{
//TMRCount < OCSetting = high... 150/200 = 75% duty cycle
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, 150);
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, 150);
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_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_TURNLEFT: //state 2, turn left.
{
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 1);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_TURNRIGHT: //state 3, turn right
{
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, 1);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_LEFTFEEDBACK:
{
motorData.Lfeedback = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_RIGHTFEEDBACK:
{
motorData.Rfeedback = motorData.rxMessage.duration;
break;
}
/* TODO: implement your application state machine.*/
case MOTOR_STATE_Straight2: //state 1, go straight.
{
//TMRCount < OCSetting = high... 150/200 = 75% duty cycle
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, 150);
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, 150);
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_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
} //end switch
}
}
else
{
crash("No Motor MsgQ");
}
}
}
void BSP_USBVBUSSwitchStateSet(BSP_USB_VBUS_SWITCH_STATE state)
{
/* Enable the VBUS switch */
PLIB_PORTS_PinWrite( PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_5, state );
}
void BSP_LEDStateSet(BSP_LED led, BSP_LED_STATE state)
{
/* Set the state of the LED */
PLIB_PORTS_PinWrite (PORTS_ID_0 , PORT_CHANNEL_H , led, state );
}