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;
