-
Notifications
You must be signed in to change notification settings - Fork 13
/
LineFollower.ino
228 lines (208 loc) · 9.89 KB
/
LineFollower.ino
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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
/*
* LineFollower.cpp
*
* Implements line follower driving for a 2 or 4 wheel car.
* A TCRT 5000 3-channel sensor is used.
*
* According to the 8 different states of the 3 sensor inputs, we perform the following actions:
* 0 - All sensors are dark or not connected -> stop or go forward after sharp turn
* 1 - Mid and right sensors are dark -> sharp right
* 2 - Left and right sensors are dark -> panic stop, because this is unexpected
* 3 - Only right sensor is dark -> right
* 4 - Mid and left sensors are dark -> sharp left
* 5 - Only mid sensor is dark -> forward
* 6 - Only left sensor is dark -> left
* 7 - All sensors are not dark -> stop or go backward after turn
*
*
* Copyright (C) 2022-2024 Armin Joachimsmeyer
* armin.joachimsmeyer@gmail.com
*
* This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl.
*
* PWMMotorControl is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/gpl.html>.
*
*/
#include <Arduino.h>
/*
* Car configuration
* For a complete list of available configurations see RobotCarConfigurations.h
* https://github.com/ArminJo/Arduino-RobotCar/blob/master/src/RobotCarConfigurations.h
*/
//#define TBB6612_4WD_4AA_BASIC_CONFIGURATION // China set with TB6612 mosfet bridge + 4 AA.
//#define TBB6612_4WD_4AA_VIN_CONFIGURATION // China set with TB6612 mosfet bridge + 4 AA + VIN voltage divider.
//#define TBB6612_4WD_4AA_FULL_CONFIGURATION // China set with TB6612 mosfet bridge + 4 AA + VIN voltage divider + MPU6050.
//#define TBB6612_4WD_4NIMH_BASIC_CONFIGURATION // China set with TB6612 mosfet bridge + 4 NiMh.
//#define TBB6612_4WD_4NIMH_VIN_CONFIGURATION // China set with TB6612 mosfet bridge + 4 NiMh + VIN voltage divider.
//#define TBB6612_4WD_2LI_ION_BASIC_CONFIGURATION // China set with TB6612 mosfet bridge + 2 Li-ion.
//#define TBB6612_4WD_2LI_ION_FULL_CONFIGURATION // China set with TB6612 mosfet bridge + 2 Li-ion + VIN voltage divider + MPU6050.
//#define L298_2WD_4AA_BASIC_CONFIGURATION // China 2WD set with L298 bridge and Uno board with series diode for VIN + 4 AA batteries. DEFAULT.
//#define L298_2WD_2LI_ION_BASIC_CONFIGURATION // China 2WD set with L298 bridge and Uno board with series diode for VIN + 2 Li-ion.
//#define L298_2WD_2LI_ION_VIN_IR_CONFIGURATION // L298_2WD_2LI_ION_BASIC + VIN voltage divider + IR distance
//#define L298_2WD_2LI_ION_VIN_IR_IMU_CONFIGURATION // L298_2WD_2LI_ION_BASIC + VIN voltage divider + IR distance + MPU6050
//#define MECANUM_US_DISTANCE_CONFIGURATION // Nano Breadboard version with Arduino NANO, TB6612 mosfet bridge and 4 mecanum wheels + US distance + servo
//
//#define TRACE
//#define DEBUG
//#define INFO
//
// Here you can override the default settings for the sensor connections
//#define LINE_FOLLOWER_LEFT_SENSOR_PIN 4
//#define LINE_FOLLOWER_MID_SENSOR_PIN 5
//#define LINE_FOLLOWER_RIGHT_SENSOR_PIN 6
#include "RobotCarConfigurations.h" // sets CAR_HAS_ENCODERS, USE_ADAFRUIT_MOTOR_SHIELD etc.
#include "RobotCarPinDefinitionsAndMore.h"
#if !defined(LINE_FOLLOWER_LEFT_SENSOR_PIN)
#error The LineFollower program requires that LINE_FOLLOWER_[LEFT,MID,RIGHT]_SENSOR_PIN are defined
#endif
/*
* Speed compensation to enable driving straight ahead.
* Is is usually not needed for a 4 wheel car.
* If positive, this value is subtracted from the speed of the right motor -> the car turns slightly right.
* If negative, -value is subtracted from the left speed -> the car turns slightly left.
*/
#define SPEED_PWM_COMPENSATION_RIGHT 0
/*
* Enable LINE_FOLLOWER_SENSORS_ARE_MOUNTED_AT_BACK_OF_CAR, if the sensors are mounted at back of your car.
* In this case, the directions must be switched, e.g. we must specify DIRECTION_BACKWARD to go forward.
*/
//#define LINE_FOLLOWER_SENSORS_ARE_MOUNTED_AT_BACK_OF_CAR
#include "CarPWMMotorControl.hpp"
#include "RobotCarUtils.hpp"
#if defined(LINE_FOLLOWER_SENSORS_ARE_MOUNTED_AT_BACK_OF_CAR)
// Here, the sensors are mounted at the back, so we must switch forward and backward directions
#define LINE_FOLLOWER_DIRECTION_FORWARD DIRECTION_BACKWARD
#define LINE_FOLLOWER_DIRECTION_BACKWARD DIRECTION_FORWARD
#define LINE_FOLLOWER_TURN_FORWARD TURN_BACKWARD
#define LINE_FOLLOWER_TURN_BACKWARD TURN_FORWARD
#else
#define LINE_FOLLOWER_DIRECTION_FORWARD DIRECTION_FORWARD
#define LINE_FOLLOWER_DIRECTION_BACKWARD DIRECTION_BACKWARD
#define LINE_FOLLOWER_TURN_FORWARD TURN_FORWARD
#define LINE_FOLLOWER_TURN_BACKWARD TURN_BACKWARD
#endif
uint8_t sOldSensorState; // Used to detect changes of sensor state
/*
* Start of line follower control program
*/
void setup() {
Serial.begin(115200);
// use INPUT_PULLUP, to avoid moving if sensor is not connected
pinMode(LINE_FOLLOWER_LEFT_SENSOR_PIN, INPUT_PULLUP);
pinMode(LINE_FOLLOWER_MID_SENSOR_PIN, INPUT_PULLUP);
pinMode(LINE_FOLLOWER_RIGHT_SENSOR_PIN, INPUT_PULLUP);
// Just to know which program is running on my Arduino
Serial.println(F("START " __FILE__ " from " __DATE__ "\r\nUsing library version " VERSION_PWMMOTORCONTROL));
// Print useful information
printConfigInfo(&Serial);
Serial.println(F("Left sensor pin=" STR(LINE_FOLLOWER_LEFT_SENSOR_PIN)));
Serial.println(F("Middle sensor pin=" STR(LINE_FOLLOWER_MID_SENSOR_PIN)));
Serial.println(F("Right sensor pin=" STR(LINE_FOLLOWER_RIGHT_SENSOR_PIN)));
/*
* Init car motor control
*/
initRobotCarPWMMotorControl();
RobotCar.setSpeedPWMCompensation(SPEED_PWM_COMPENSATION_RIGHT); // Set left/right speed compensation
RobotCar.setStopMode(STOP_MODE_RELEASE); // to enable more smooth braking
/*
* Tone feedback for end of boot
*/
tone(BUZZER_PIN, 2200, 100);
/*
* Do not start immediately with driving
*/
delay(3000);
}
void loop() {
/*
* Get values from the TCRT 5000 3-channel sensor
* input is high, if sensor sees dark
*/
uint8_t tNewSensorState = 0;
if (digitalRead(LINE_FOLLOWER_LEFT_SENSOR_PIN) == LOW) tNewSensorState = 1; // set bit 0 if input is LOW
if (digitalRead(LINE_FOLLOWER_MID_SENSOR_PIN) == LOW) tNewSensorState |= 2; // set bit 1 if input is LOW
if (digitalRead(LINE_FOLLOWER_RIGHT_SENSOR_PIN) == LOW) tNewSensorState |= 4; // set bit 2 if input is LOW
/*
* According to the 8 different states of the 3 sensor inputs, we perform the following actions:
* 0 - All sensors are dark or not connected -> stop or go forward after sharp turn
* 1 - Mid and right sensors are dark -> sharp right
* 2 - Left and right sensors are dark -> panic stop, because this is unexpected
* 3 - Only right sensor is dark -> right
* 4 - Mid and left sensors are dark -> sharp left
* 5 - Only mid sensor is dark -> forward
* 6 - Only left sensor is dark -> left
* 7 - All sensors are not dark -> stop or go backward after turn
*/
/*
* If sensor input does not change, we do not need to change movement!
*/
if (sOldSensorState != tNewSensorState) {
Serial.print(F("SensorState="));
Serial.print(tNewSensorState);
Serial.print(F(" -> "));
switch (tNewSensorState) {
case 0:
// All sensors are dark or not connected -> stop or go forward after sharp turn
if (sOldSensorState == 1 || sOldSensorState == 4) { // test for sharp turns
RobotCar.setSpeedPWMAndDirection(DEFAULT_DRIVE_SPEED_PWM, LINE_FOLLOWER_DIRECTION_FORWARD);
Serial.println(F("Forward during sharp turn"));
} else {
RobotCar.stop();
Serial.println(F("Stop"));
}
break;
case 1:
// Mid and right sensors are dark -> sharp right
RobotCar.startRotate(-180, TURN_IN_PLACE);
Serial.println(F("Turn sharp right"));
break;
case 2:
// Left and right sensors are dark -> panic stop, because this is unexpected
RobotCar.stop();
Serial.println(F("panic stop"));
break;
case 3:
// Only right sensor is dark -> right
RobotCar.startRotate(-180, LINE_FOLLOWER_TURN_FORWARD);
Serial.println(F("Turn right"));
break;
case 4:
// Mid and left sensors are dark -> sharp left
RobotCar.startRotate(180, TURN_IN_PLACE);
Serial.println(F("Turn sharp left"));
break;
case 5:
// Only mid sensor is dark -> forward
RobotCar.setSpeedPWMAndDirection(DEFAULT_DRIVE_SPEED_PWM, LINE_FOLLOWER_DIRECTION_FORWARD);
Serial.println(F("Forward"));
break;
case 6:
// Only left sensor is dark -> left
RobotCar.startRotate(180, LINE_FOLLOWER_TURN_FORWARD);
Serial.println(F("Turn left"));
break;
case 7:
// All sensors are not dark -> stop or go backward after turn
if (sOldSensorState == 6 || sOldSensorState == 3) {
RobotCar.setSpeedPWMAndDirection(DEFAULT_DRIVE_SPEED_PWM, LINE_FOLLOWER_DIRECTION_BACKWARD);
Serial.println(F("Forward after turn"));
} else {
RobotCar.stop();
Serial.println(F("Stop"));
}
break;
}
sOldSensorState = tNewSensorState;
}
}