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();*/
}
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");
}
}
}