forked from DerekQXu/MicroMouse_2017
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Last Year.cpp
428 lines (360 loc) · 11 KB
/
Last Year.cpp
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
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
#include "mbed.h"
#include "QEI.h"
// wait function has an input unit of seconds, with precision of float.
void forward(int);
void rest(float);
void turnLeft(int);
void turnRight(int);
void resetEncoders();
void printGyro();
void printSensors();
void printSensorsCont();
void calibration(float& cL, float& cR, float& cFL, float& cFR);
void PID( float InputL, float InputR, float& speedL, float& speedR, const float kp , const float ki, const float kd);
float map ( float& var, float& imin, float& imax, float& omin, float& omax);
void move();
const float SPEED = 0.4f;
const float TURNSPEED = 0.4f;
const float WHEEL_RATIO = 0.95f;
const float SLOW = SPEED * WHEEL_RATIO;
const float CELL = 18; // Length for 1 Cell.
const float TURN = 1.333815f; // DISTANCE for a TURN of 45 deg.
const float TURNERRORL = 1.94740f;
const float TURNERRORR = 1.97644f; // 1.75655// 1.84668
const float HOLD = 0.001f;
const float dPerCountR = 0.010279f;
const float dPerCountL = 0.010272f;
const int samples = 1000;
// For Moving
float SetpointL; // run calibration function
float SetpointR; // run calibration function
float SetpointFL; // run calibration function
float SetpointFR; // run calibration function
float distanceR, distanceL, distanceFR, distanceFL;
float calibratorR, calibratorL, calibratorFR, calibratorFL;
float InputR, InputL;
float defaultSpeed = SPEED;
float speedL = SPEED, speedR = SPEED;
float pError=0;
const float Kp = 5;
const float Kd = 8;
const float Ki = 0;
DigitalOut A (PC_10);
DigitalOut B (PC_11);
DigitalOut C (PB_0);
DigitalOut D (PB_7);
PwmOut LMF (PA_7);
PwmOut RMF (PB_10);
PwmOut LMB (PB_6);
PwmOut RMB (PC_7);
AnalogIn gyro(PC_2);
AnalogIn frontLeft(PC_1);
AnalogIn frontRight(PA_4);
AnalogIn sideLeft(PC_0);
AnalogIn sideRight(PA_0);
Serial pc(PA_2, PA_3);
QEI leftWheel(PA_1, PC_4, NC, 624, QEI::X4_ENCODING);
QEI rightWheel(PA_15, PB_3, NC, 624, QEI::X4_ENCODING);
int main()
{
pc.printf("\nStart!\n");
A = 1;
B = 1;
C = 1;
D = 1;
//printSensorsCont();
wait(2);
calibration(calibratorL, calibratorR, calibratorFL, calibratorFR);
SetpointL = calibratorL;
SetpointR = calibratorR;
SetpointFL = calibratorFL;
SetpointFR = calibratorFR;
for(;;)
{
distanceR = sideRight.read();
distanceL = sideLeft.read();
distanceFR = frontRight.read();
distanceFL = frontLeft.read();
printSensorsCont();
/*
if (distanceR >= 0.3 && distanceL <= 0.3)
{
rest(1);
turnLeft(2);
}
*/
if (distanceFR >= 0.6 && distanceFL >= 0.6 && distanceR > distanceL )
{
rest(1);
turnLeft(1);
}
/*
if (distanceR >= 0.3 && distanceL <= 0.3)
{
rest(1);
turnRight(2);
}
*/
if (distanceFR >= 0.6 && distanceFL >= 0.6 && distanceL > distanceR )
{
rest(1);
turnRight(1);
}
if (distanceFR >= 0.6 && distanceFL >= 0.6 && distanceR >= 0.85 && distanceL >= 0.85 )
{
rest(1);
turnLeft(2);
}
// Map raw sensor values to between 0 and 255
// Set values to input to PID
InputL = distanceL-SetpointL;
InputR = distanceR-SetpointR;
// mapping, cuz the max speed motor can have is 0.4f
InputL = SPEED * InputL;
InputR = SPEED * InputR;
PID( InputL, InputR, speedL, speedR, Kp , Ki, Kd);
// move
move();
}
pc.printf("End!\n");
}
// move forward number of cells
// WORKS!
void forward(int cells)
{
resetEncoders();
pc.printf("Moving forward %i cells\n", cells);
float distance = CELL*cells;
int leftPulses = 0;
int rightPulses = 0;
LMB = 0;
RMF = SPEED;
LMF = SPEED;
RMB = 0;
// Old way of doing things (without serial):
// for (float s = 0; s < count; s+= HOLD) {
// Misheel's code starts here (with serial):
// 1 revolution is approximated to be ~400 counts.
for (;;){
leftPulses = leftWheel.getPulses();
rightPulses = rightWheel.getPulses();
// Somehow the signs were random each time I do things
// sometimes plus sometimes negative
// so just made everything plus
if (leftPulses < 0 )
leftPulses = (-1) * leftPulses;
if (rightPulses < 0 )
rightPulses = (-1) * rightPulses;
pc.printf("Left: %i\tRight: %i\n", leftPulses, rightPulses);
// pc.printf("Gyro: %f\n", gyro.read() * 100.0f);
// If sufficient distance traveled, break.
if ((leftPulses * dPerCountL) >= distance || rightPulses * dPerCountR >= distance)
break;
wait(HOLD);
}
pc.printf("\n");
resetEncoders();
}
// Rest for some seconds
void rest(float sec)
{
LMF = 0;
RMF = 0;
LMB = 0;
RMB = 0;
wait(sec);
}
void turnLeft(int times)
{
resetEncoders();
pc.printf("Turning left %i times\n", times);
float distance;
// this is for 180 deg tuning:
if (times == 4)
distance = TURN * times * TURNERRORL;
else
distance = TURN * times;
int leftPulses = 0;
int rightPulses = 0;
LMB = TURNSPEED;
RMF = TURNSPEED;
LMF = 0;
RMB = 0;
for (;;) {
leftPulses = leftWheel.getPulses();
rightPulses = rightWheel.getPulses();
// Somehow the signs were random each time I do things
// sometimes plus sometimes negative
// so just made everything plus
if (leftPulses < 0 )
leftPulses = (-1) * leftPulses;
if (rightPulses < 0 )
rightPulses = (-1) * rightPulses;
pc.printf("Left: %i\tRight: %i\n", leftPulses, rightPulses);
pc.printf("Gyro: %f\n", gyro.read() * 100.0f);
if ((leftPulses * dPerCountL) >= distance || rightPulses * dPerCountR >= distance)
break;
}
pc.printf("\n");
resetEncoders();
}
void turnRight(int times)
{
resetEncoders();
pc.printf("Turning right %i times\n", times);
float distance;
// for 180 deg tuning
if (times == 4)
distance = TURN * times * TURNERRORR;
else
distance = TURN * times;
int leftPulses = 0;
int rightPulses = 0;
LMB = 0;
RMF = 0;
LMF = TURNSPEED;
RMB = TURNSPEED;
for (;;) {
leftPulses = leftWheel.getPulses();
rightPulses = rightWheel.getPulses();
// Somehow the signs were random each time I do things
// sometimes plus sometimes negative
// so just made everything plus
if (leftPulses < 0 )
leftPulses = (-1) * leftPulses;
if (rightPulses < 0 )
rightPulses = (-1) * rightPulses;
pc.printf("Left: %i\tRight: %i\n", leftPulses, rightPulses);
pc.printf("Gyro: %f\n", gyro.read() * 100.0f);
if ((leftPulses * dPerCountL) >= distance || rightPulses * dPerCountR >= distance)
break;
}
pc.printf("\n");
resetEncoders();
}
// resets encoders
// reseting at the beginning and the end
void resetEncoders()
{
rightWheel.reset();
leftWheel.reset();
}
void printGyro()
{
for(;;)
{
pc.printf("Gyro: %f\n", gyro.read() * 1.0f);
wait(0.1);
}
}
void printSensors()
{
float Left = sideLeft.read() ;
float FLeft = frontLeft.read() ;
float FRight = frontRight.read() ;
float Right = sideRight.read();
pc.printf("Left: %f\tFLeft: %f\t\tFRight: %f\tRight: %f\n",
Left, FLeft, FRight, Right);
}
void printSensorsCont()
{
for(;;)
{
printSensors();
}
}
int P_Controller (int error)
{
int correction = Kp * error;
return correction;
}
void calibration(float& cL, float& cR, float& cFL, float& cFR)
{
float TcR = 0;
float TcL = 0;
float TcFR = 0;
float TcFL = 0;
for(int i= 0; i < samples; i++)
{
TcR += sideRight.read();
TcL += sideLeft.read();
TcFR += frontRight.read();
TcFL += frontLeft.read();
}
TcR /= samples;
TcL /= samples;
TcFR /= samples;
TcFL /= samples;
cR = TcR;
cL = TcL;
cFR = TcR;
cFL = TcL;
}
void PID( float InputL, float InputR, float& speedL, float& speedR, const float kp , const float ki, const float kd)
{
/*Compute all the working error variables*/
float tspeedL = speedL;
float tspeedR = speedR;
float tError = InputR - InputL;
float derivation = tError - pError;
float correction = kp*tError + kd*derivation;
tspeedR = defaultSpeed + correction;
tspeedL = defaultSpeed - correction;
if (tspeedR > SPEED)
speedR = SPEED;
if (tspeedL > SPEED)
speedL = SPEED;
if (tspeedL < 0)
speedR = 0;
if (tspeedL < 0)
speedL = 0;
speedL = tspeedL;
speedR = tspeedR;
pError = tError;
}
float map (float& var, float& imin, float& imax, float& omin, float& omax)
{
return var = omax / imax * var;
}
void move()
{
LMB = 0;
RMF = speedR;
LMF = speedL;
RMB = 0;
}
void reverse(int cells)
{
resetEncoders();
pc.printf("Moving forward %i cells\n", cells);
float distance = CELL*cells;
int leftPulses = 0;
int rightPulses = 0;
LMB = 0;
RMF = SPEED;
LMF = SPEED;
RMB = 0;
// Old way of doing things (without serial):
// for (float s = 0; s < count; s+= HOLD) {
// Misheel's code starts here (with serial):
// 1 revolution is approximated to be ~400 counts.
for (;;){
leftPulses = leftWheel.getPulses();
rightPulses = rightWheel.getPulses();
// Somehow the signs were random each time I do things
// sometimes plus sometimes negative
// so just made everything plus
if (leftPulses < 0 )
leftPulses = (-1) * leftPulses;
if (rightPulses < 0 )
rightPulses = (-1) * rightPulses;
pc.printf("Left: %i\tRight: %i\n", leftPulses, rightPulses);
pc.printf("Gyro: %f\n", gyro.read() * 100.0f);
// If sufficient distance traveled, break.
if ((leftPulses * dPerCountL) >= distance || rightPulses * dPerCountR >= distance)
break;
wait(HOLD);
}
pc.printf("\n");
resetEncoders();
}