-
Notifications
You must be signed in to change notification settings - Fork 0
/
manual_mode.ino
125 lines (81 loc) · 2.34 KB
/
manual_mode.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
/* A simple Manual Steering Mode
* Juni Adi, July 6th 2020
*
*/
void manualMode(int rcThrottle, int rcSteer) {
Serial.println("Manual");
int lThrottle;
int rThrottle;
// failsafe
if (rcThrottle == 0 & rcSteer == 0 ) {
mStop();
}
// move forward
else if (rcThrottle > 1500 ) {
rcThrottle = map(rcThrottle, 1500, 2000, 0, 255);
if (rcSteer > 1500) { // forward turn right
rThrottle = map(rcSteer, 1500, 2000, rcThrottle, 0);
rMotor.setSpeed(rThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(FORWARD);
lMotor.run(FORWARD);
}
else if (rcSteer < 1500 ) { // FORWARD TURN LEFT
lThrottle = map(rcSteer, 1500, 1000, rcThrottle, 0);
lMotor.setSpeed(lThrottle);
rMotor.setSpeed(rcThrottle);
rMotor.run(FORWARD);
lMotor.run(FORWARD);
}
else { // forward turn left
rMotor.setSpeed(rcThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(FORWARD);
lMotor.run(FORWARD);
}
}
// BACKWARD
else if (rcThrottle < 1500 && rcThrottle > 0) {
rcThrottle = map(rcThrottle, 1500, 1000, 0, 255);
if (rcSteer > 1500) { // backward turn right
lThrottle = map(rcSteer, 1500, 2000, rcThrottle, 0);
lMotor.setSpeed(lThrottle);
rMotor.setSpeed(rcThrottle);
rMotor.run(BACKWARD);
lMotor.run(BACKWARD);
}
else if (rcSteer < 1500 ) { // backward turn left
rThrottle = map(rcSteer, 1500, 1000, rcThrottle, 0);
rMotor.setSpeed(rThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(BACKWARD);
lMotor.run(BACKWARD);
}
else { // backward straight
rMotor.setSpeed(rcThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(BACKWARD);
lMotor.run(BACKWARD);
}
}
// IDDLE
else {
if (rcSteer > 1500) { // rotate right
rcThrottle = map(rcSteer, 1500, 2000, 0, 255);
rMotor.setSpeed(rcThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(BACKWARD);
lMotor.run(FORWARD);
}
else if (rcSteer < 1500) { // rotate left
rcThrottle = map(rcSteer, 1500, 1000, 0, 255);
rMotor.setSpeed(rcThrottle);
lMotor.setSpeed(rcThrottle);
rMotor.run(FORWARD);
lMotor.run(BACKWARD);
}
else {
mStop();
}
}
}