static void vTaskVehicleCtrl(void *pvParameters) { float heading; while (1) { if (bVehicleAuto) { __WFI(); int angle = angle_subtract(heading, CompassGetHeading()); if (angle < 90) { MotorRun(MOTOR_LEFT , iVehicleMotorDutyL); MotorRun(MOTOR_RIGHT, -iVehicleMotorDutyR); continue; } if (angle > 100) { MotorRun(MOTOR_LEFT , -iVehicleMotorDutyL); MotorRun(MOTOR_RIGHT, iVehicleMotorDutyR); continue; } MotorRun(MOTOR_LEFT , iVehicleMotorDutyL); MotorRun(MOTOR_RIGHT, iVehicleMotorDutyR); bVehicleAuto = false; } else { Board_LED_Toggle(0); float obstacle = UltrasonicPing(); if (obstacle < THRESHOLD_AUTODRIVE) { Board_LED_Set(0, false); bVehicleAuto = true; // FIXME heading = CompassGetHeading(); MotorRun(MOTOR_LEFT , MAX_PWM_CYCLE / 10); MotorRun(MOTOR_RIGHT, -MAX_PWM_CYCLE / 10); } else { /* toggle rate based on the range to the obstacle */ vTaskDelay(200); //beatrate(UltrasonicPing())); } } } }
/*lint -save -e970 Disable MISRA rule (6.3) checking. */ int main(void) /*lint -restore Enable MISRA rule (6.3) checking. */ { /* Write your local variable definition here */ unsigned char i; unsigned char MotorToUpdate=0; /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/ PE_low_level_init(); /*** End of Processor Expert internal initialization. ***/ /* Write your code here */ Led_green_SetVal(); Led_blue_SetVal(); Led_red_SetVal(); PCA9685Config(); PCA9685EnableOutput(1); CompassInit(); compassCalibrationReady=0; while(1){ // BOUSSOLE, LECTURE DE L'ANGLE D'AZIMUTH, CALIBRAGE SI BESOIN if(compassCalibrationReady){ // Contrôle si calibration nécéssaire if(compassEnable && trigCompassRead){ // Boussole active et 300mS ecoulée ? if(CompassCheck()){ myHeading=CompassGetHeading(); // Lecture de l'angle azimuth // CONTROLE SI INTERRUPTION HEADING if(compassInterruptEnable){ if((myHeading>=compassInterruptMin) && (myHeading<=compassInterruptMax)) compassInterruptFlag=1; else compassInterruptFlag=0; } compassDataValid=TRUE; trigCompassRead=0; } } Led_blue_SetVal(); } else{ if(compassEnable){ CompassCalibrate(); // Calibration myHeading=0xFFFF; compassDataValid=FALSE; Led_blue_ClrVal(); } } // COMMUNICATION AVEC MASTER, TRAITEMENT DE LA COMMANDE UART RECUE if(UARTDataReady){ processCommand(); UARTDataReady=0; } // MESURE DISTANCE ULTRASON, LECTURE DE LA DISTANCE if(UltrasonicDataReady){ UltrasonicDistance_mm=((UltrasonicEchoTime*10)/58)*10; uSonicDataValid=1; UltrasonicDataReady=0; } // BOUSSOLE, INTERRUPTION PROVOQUEE PAR UN ANGLE DEFINI, AVERTISSEMENT AU MASTER if(compassInterruptFlag){ if(!compassInterruptSended){ buffRcvDcfData[0]=INTERRUPT_COMPASS; buffRcvDcfData[1]=compassDataValid; buffRcvDcfData[2]=(myHeading & 0xFF00)>>8; buffRcvDcfData[3]=myHeading & 0x00FF; DataLength=4; replyToRemote(); compassInterruptSended=1; } } else compassInterruptSended=0;