Détection d'obstacle⚓

A partir de de la documentation du constructeur mettre en œuvre la détection d'obstacle du robot.

1
#define EN1 6//pin for run the right motor
2
#define IN1 7//pin for control right motor direction
3
#define EN2 5//pin for run the left motor
4
#define IN2 12//pin for control left motor direction
5
6
#define FORW 0//go forward
7
#define BACK 1//go back
8
9
#define IR_IN  17//IR receiver pin
10
#define L_IR 13//left ir transmitter pin
11
#define R_IR 8//right ir transmitter pin
12
13
volatile int count;//count the motor speed pulse
14
15
void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN)//control motor
16
{
17
  //////////M1////////////////////////
18
  if(M1_DIR==FORW)//M1 motor direction
19
    digitalWrite(IN1,FORW);//forward
20
  else
21
    digitalWrite(IN1,BACK);//back
22
  if(M1_EN==0)
23
    analogWrite(EN1,LOW);//stop
24
  else
25
    analogWrite(EN1,M1_EN);//set speed
26
27
  ///////////M2//////////////////////
28
  if(M2_DIR==FORW)
29
    digitalWrite(IN2,FORW);
30
  else
31
    digitalWrite(IN2,BACK);
32
  if(M2_EN==0)
33
    analogWrite(EN2,LOW);
34
  else
35
    analogWrite(EN2,M2_EN);
36
}
37
38
void L_Send40KHZ(void)//left ir transmitter sends 40kHZ pulse
39
{
40
  int i;
41
  for(i=0;i<24;i++)
42
  {
43
    digitalWrite(L_IR,LOW);
44
    delayMicroseconds(8);
45
    digitalWrite(L_IR,HIGH);
46
    delayMicroseconds(8);
47
  }
48
}
49
void R_Send40KHZ(void)//right ir transmitter sends 40kHZ pulse
50
{
51
  int i;
52
  for(i=0;i<24;i++)
53
  {
54
    digitalWrite(R_IR,LOW);
55
    delayMicroseconds(8);
56
    digitalWrite(R_IR,HIGH);
57
    delayMicroseconds(8);
58
  }
59
}
60
void pcint0_init(void)//init the interrupt
61
{
62
  PCICR = 0X01;//
63
  PCMSK0 = 0X01;//
64
}
65
66
ISR(PCINT0_vect)//motor encoder interrupt
67
{
68
  count++;
69
}
70
void Obstacle_Avoidance(void)
71
{
72
  char i;
73
  count=0;
74
  for(i=0;i<20;i++)//left transmitter sends 20 pulses
75
  {
76
    L_Send40KHZ();
77
    delayMicroseconds(600);    
78
  }
79
  if(count>20)//if recieved a lot pulse , it means there's a obstacle
80
  {
81
    Motor_Control(BACK,100,BACK,100);
82
    delay(300);
83
    Motor_Control(BACK,100,FORW,100);
84
    delay(500);
85
  }
86
  else
87
  {
88
    count=0;
89
    for(i=0;i<20;i++)//right transmitter sends 20 pulses
90
    {
91
      R_Send40KHZ(); 
92
      delayMicroseconds(600);        
93
    }
94
    if(count>20)
95
    {
96
      Motor_Control(BACK,100,BACK,100);
97
      delay(300);
98
      Motor_Control(FORW,100,BACK,100);
99
      delay(500);
100
    }
101
    else
102
    {
103
      Motor_Control(FORW,100,FORW,100);
104
    }
105
  }
106
}
107
void setup()
108
{
109
  pinMode(5,OUTPUT);//init the motor driver pins
110
  pinMode(6,OUTPUT);
111
  pinMode(7,OUTPUT);
112
  pinMode(12,OUTPUT);
113
  
114
  pinMode(L_IR,OUTPUT);//init the left transmitter pin
115
  pinMode(R_IR,OUTPUT);//init the right transmitter pin
116
  pinMode(IR_IN,INPUT);//init the ir receiver pin
117
  
118
  digitalWrite(R_IR,HIGH);
119
  digitalWrite(L_IR,HIGH);
120
  pcint0_init();
121
  sei();               //enable the interrupt
122
}
123
void loop()
124
{
125
  Motor_Control(FORW,100,FORW,100);//run motor
126
  while(1)
127
  {
128
    Obstacle_Avoidance();//obstacle avoidance
129
  }
130
}
131
A partir des test et de la documentation, expliquer précisément comment s'opère la détection d'un obstacle
Quelle stratégie au niveau de la commande moteur est adoptée ?