1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
|
#include <stdbool.h>
#include "lib_mot_moway.h"
#include "lib_sen_moway.h"
bool is_forward_obstacle(void)
{
return SEN_OBS_ANALOG(OBS_SIDE_L)>100
|| SEN_OBS_ANALOG(OBS_SIDE_R)>100
|| SEN_OBS_ANALOG(OBS_CENTER_L)>100
|| SEN_OBS_ANALOG(OBS_CENTER_R)>100
;
}
void turn_back(void)
{
MOT_ROT(100,FWD,CENTER,RIGHT,ANGLE,50);
while (!MOT_END)
; // attendre fin rotation
}
void stop(void)
{
MOT_STOP();
}
void main(void)
{
//Appelé au RESET
SEN_CONFIG();
MOT_CONFIG();
while (1) {
// Faire avancer le robot sur la ligne noire
if (SEN_LINE_DIG(LINE_L)==1 && SEN_LINE_DIG(LINE_R)==1) {
MOT_STR(50,FWD,TIME,0);
}
// Faire dévier le robot vers la gauche quand capteurs de ligne droit voit du blanc
if (SEN_LINE_DIG(LINE_R)==0 && SEN_LINE_DIG(LINE_L)==1) {
MOT_ROT(50,FWD,CENTER,LEFT,ANGLE,10);
}
// Faire dévier le robot vers la droite quand capteur ligne gauche voit du blanc
if (SEN_LINE_DIG(LINE_R)==1 && SEN_LINE_DIG(LINE_L)==0) {
MOT_ROT(50,FWD,CENTER,RIGHT,ANGLE,10);
}
// gestion obstacle
if (is_forward_obstacle()) {
stop();
int wait_limit= 10;
for (int wait_count=0; wait_count <= wait_limit && is_forward_obstacle(); ++wait_count) {
if (wait_count == wait_limit)
turn_back();
else
PAUSE_SECONDE(1);
}
}
}
} |
Partager