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); }