Пример #1
0
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();*/

}
Пример #2
0
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");
		}
	}
}