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 DRV_TMR_Stop ( DRV_HANDLE handle )
{
switch (handle)
{
case DRV_TMR_INDEX_0:
{
DRV_TMR0_Stop();
break;
}
default:
{
break;
}
}
}
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");
}
}
}