-
Notifications
You must be signed in to change notification settings - Fork 0
/
Line_following_smooth.ino
258 lines (191 loc) · 6.61 KB
/
Line_following_smooth.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
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
#include "encoders.h"
# define L_PWM_PIN 10
# define L_DIR_PIN 16
# define R_PWM_PIN 9
# define R_DIR_PIN 15
// Set initial direction (HIGH/LOW)
// for the direction pins.
# define FWD LOW
# define REV HIGH
# define EMIT_PIN 11 // IR emitter pin
# define LS_LEFT_PIN 12 // Line sensor DN1 pin
# define LS_MIDLEFT_PIN 18 // Line sensor DN2 pin
# define LS_MIDDLE_PIN 20 // Line sensor DN3 pin
# define LS_MIDRIGHT_PIN 21 // Line sensor DN4 pin
# define LS_RIGHT_PIN 22 // Line sensor DN5 pin
# define MAX_SAMPLES 10
unsigned long results[ MAX_SAMPLES ]; // An array of MAX_SAMPLES length
int ls_pins[5] = {LS_LEFT_PIN,
LS_MIDLEFT_PIN,
LS_MIDDLE_PIN,
LS_MIDRIGHT_PIN,
LS_RIGHT_PIN };
//demographics of the robot
const int CLICKS_PER_ROTATION = 12;
const float GEAR_RATIO = 29.86F;
const float WHEEL_DIAMETER = 32;
const int WHEEL_CIRCUMFERENCE = 100.48;
// Variables to store the current (x, y) coordinates
float currentX = 0.0;
float currentY = 0.0;
float currentTheta = 0.0; // Current orientation angle (in radians)
// Function to update the (x, y) coordinates based on encoder data
void updateCoordinates() {
// Calculate the distance traveled by both wheels
float leftDistance = ((-count_e1) * WHEEL_CIRCUMFERENCE) / (CLICKS_PER_ROTATION * GEAR_RATIO);
float rightDistance = ((-count_e0) * WHEEL_CIRCUMFERENCE) / (CLICKS_PER_ROTATION * GEAR_RATIO);
// Calculate the average distance
float avgDistance = (leftDistance + rightDistance) / 2.0;
// Update the robot's orientation (theta)
float deltaTheta = (leftDistance - rightDistance) / WHEEL_DIAMETER;
currentTheta += deltaTheta;
// Update the (x, y) coordinates based on the distance and orientation
currentX += avgDistance * cos(currentTheta);
currentY += avgDistance * sin(currentTheta);
// Reset the encoder counts to zero
count_e1 = 0; //left
count_e0 = 0; //right
}
//function for motor
void setMotorPower( float left_pwm, float right_pwm ) {
if(left_pwm>0){
digitalWrite(L_DIR_PIN, FWD);
analogWrite( L_PWM_PIN, left_pwm );
}
else{
analogWrite( L_PWM_PIN, -left_pwm );
digitalWrite(L_DIR_PIN, REV);
}
if(right_pwm>0){
digitalWrite(R_DIR_PIN, FWD);
analogWrite( R_PWM_PIN, right_pwm );
}
else{
digitalWrite(R_DIR_PIN, REV);
analogWrite( R_PWM_PIN, -right_pwm );
}
}
void driveForward() {
// Reset the encoder counts to zero
float lineTouchingDistance = 220; // millimeters
count_e0 = 0;
count_e1 = 0;
// Calculate the number of encoder counts needed to travel the target distance
float countsNeeded = (lineTouchingDistance / WHEEL_CIRCUMFERENCE) * CLICKS_PER_ROTATION * GEAR_RATIO;
//float countsNeeded = 660;
// Drive forward until the target distance is reached
while (-count_e0 < countsNeeded || -count_e1 < countsNeeded) {
setMotorPower(17, 22);
}
// Stop the motors
setMotorPower(0, 0);
}
void turnToOrigin() {
// Calculate the angle (theta) to point the robot toward the origin
float targetTheta = atan2(-currentY, -currentX);
// Calculate the change in angle needed
float deltaTheta = targetTheta - currentTheta;
// Set a rotation speed (you can adjust this)
// float rotationSpeed = 20; // Adjust as needed
// Determine the direction of the turn (clockwise or counterclockwise)
int turnDirection = (deltaTheta > 0) ? 1 : -1;
// Perform the turn
while (deltaTheta > 0.05) { // Adjust the threshold for precision
setMotorPower(turnDirection * 17, -turnDirection * 20);
updateCoordinates(); // Update orientation based on encoder data
deltaTheta = targetTheta - currentTheta;
}
// Stop the motors
setMotorPower(0, 0);
}
void setup() {
// Set some initial pin modes and states
pinMode( EMIT_PIN, INPUT ); // Set EMIT as an input (off)
pinMode( LS_LEFT_PIN, INPUT ); // Set line sensor pin to input
pinMode( LS_MIDLEFT_PIN, INPUT );
pinMode( LS_MIDDLE_PIN, INPUT );
pinMode( LS_MIDRIGHT_PIN, INPUT );
pinMode( LS_RIGHT_PIN, INPUT );
pinMode(L_PWM_PIN, OUTPUT);
pinMode(L_DIR_PIN, OUTPUT);
pinMode(R_PWM_PIN, OUTPUT);
pinMode(R_DIR_PIN, OUTPUT);
// Set initial power values for the PWM pins
analogWrite(L_PWM_PIN, 0);
analogWrite(R_PWM_PIN, 0);
//initiating encoders
setupEncoder0();
setupEncoder1();
// Start Serial, wait to connect, print a debug message.
Serial.begin(9600);
delay(2000);
Serial.println("***RESET***");
//driveForward();
} // End of setup()
float readLineSensor( int i ) {
unsigned long elapsed_time=0;
if(i>=0 && i<5){
pinMode(EMIT_PIN, OUTPUT);
digitalWrite(EMIT_PIN, HIGH);
pinMode(ls_pins[i], OUTPUT);
digitalWrite(ls_pins[i], HIGH );
delayMicroseconds(10);
pinMode(ls_pins[i], INPUT);
unsigned long start_time = micros();
while(digitalRead(ls_pins[i] ) == HIGH ) {
//Do nothing here(waiting)
}
unsigned long end_time=micros();
pinMode(EMIT_PIN, INPUT);
elapsed_time = end_time - start_time;
}
return (float)elapsed_time;
}
float weightedMeasurement(){
float r1, r3;
r1 = readLineSensor(1);
r3 = readLineSensor(3);
float sum = r1 + r3;
r1 = r1 / sum;
r3 = r3 / sum;
r1 = r1 * 2.0;
r3 = r3 * 2.0;
float w = r3 - r1;
return w;
}
boolean OnLine(){
if(readLineSensor(1)>=1000 || readLineSensor(2)>=1300 || readLineSensor(3)>=1000){return true;}
else{return false;}
}
boolean OnLineButTakeLeft(){
if(readLineSensor(1)>=1100 || readLineSensor(2)>=1100){return true;}
else{return false;}
}
boolean OnLineButTakeRight(){
if(readLineSensor(3)>=1100 || readLineSensor(2)>=1101){return true;}
else{return false;}
}
void rotate(){
setMotorPower(-17,23);
}
void linefollowing(){
int BiasPWM = 22;
int MaxTurnPWM = 40;
int leftMinPWM = 17;
int rightMinPWM = 23;
if(OnLine()== true){
float W = weightedMeasurement();
float LeftPWM = BiasPWM +(W*MaxTurnPWM);
float RightPWM = BiasPWM - (W*MaxTurnPWM);
setMotorPower(LeftPWM, RightPWM);
}
else if(OnLineButTakeLeft() == true){setMotorPower(-leftMinPWM,(rightMinPWM));} //anti-clockwise
else if(OnLineButTakeRight() == true){setMotorPower(leftMinPWM,-(rightMinPWM));} //clockwise
else{
rotate();
}
}
void loop() {
linefollowing();
}
// End of loop()