-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathcontrolmegaWDP2.ino
178 lines (165 loc) · 5.32 KB
/
controlmegaWDP2.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
#include <Servo.h>
//Receiver arduino pins
const int pin4 = 4;
const int pin2 = 2;
const int pin3 = 3;
const int pin5 = 5;
//Receiver channels
float channel4;
float channel2;
float channel3;
float channel5;
//Servos and thrusters
Servo servoRight;
Servo servoLeft;
Servo thrusterRearRight;
Servo thrusterRearLeft;
Servo thrusterFrontRight;
Servo thrusterFrontLeft;
void setup() {
//Pin modes
pinMode(pin4, INPUT);
pinMode(pin2, INPUT);
pinMode(pin3, INPUT);
pinMode(pin5, INPUT);
servoRight.attach(6);
servoLeft.attach(7);
thrusterRearRight.attach(10);
thrusterRearLeft.attach(11);
thrusterFrontRight.attach(8);
thrusterFrontLeft.attach(9);
//Stop thrusters
thrusterRearRight.writeMicroseconds(1500);
thrusterRearLeft.writeMicroseconds(1500);
thrusterFrontRight.writeMicroseconds(1500);
thrusterFrontLeft.writeMicroseconds(1500);
//Driver setup
delay(1000);
Serial.begin(9600);
}
void read_values () {
//Read channel frequecies
channel4 = pulseIn(pin4, HIGH);
channel2 = pulseIn(pin2, HIGH);
channel3 = pulseIn(pin3, HIGH);
channel5 = pulseIn(pin5, HIGH);
}
void select(){
//Use channel 5 to select current mode
if (channel5 > 1600){
move_2();
}
else if (channel5 < 1400){
power_Difference();
}
else{
autonomous_Mode();
}
}
void move_1 (){ //Remote control movement
if (channel4 > 1470 & channel4 < 1530){
//Servos at the middle
servoRight.write(90);
servoLeft.write(90);
}
else {
//Servo movement relation
servoRight.write(map(channel4, 2000, 950, 0, 180));
servoLeft.write(map(channel4, 2000, 950, 0, 180));
}
if (channel3 > 1470 & channel3 < 1530) {
//Range to stop rear thrusters
thrusterRearRight.writeMicroseconds(1500);
thrusterRearLeft.writeMicroseconds(1500);
}
else {
//Rear thrusters signal
thrusterRearRight.writeMicroseconds(map(channel3, 988, 2012, 1100, 1900));
thrusterRearLeft.writeMicroseconds(map(channel3, 988, 2012, 1100, 1900));
}
if (channel2 > 1470 & channel2 < 1530) {
//Range to stop front thrusters
thrusterFrontRight.writeMicroseconds(1500);
thrusterFrontLeft.writeMicroseconds(1500);
}
else {
//Front thrusters signal
thrusterFrontRight.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterFrontLeft.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
}
}
void move_2 (){ //Remote control movement
if (channel4 > 1470 & channel4 < 1530){
//Servos at the middle
servoRight.write(90);
servoLeft.write(90);
}
else {
//Servo movement relation
servoRight.write(map(channel4, 2000, 950, 0, 180));
servoLeft.write(map(channel4, 2000, 950, 0, 180));
}
if (channel2 > 1470 & channel2 < 1530) {
//Range to stop thrusters
thrusterFrontRight.writeMicroseconds(1500);
thrusterFrontLeft.writeMicroseconds(1500);
thrusterRearRight.writeMicroseconds(1500);
thrusterRearLeft.writeMicroseconds(1500);
}
else {
//Thrusters signal
thrusterFrontRight.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterFrontLeft.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterRearRight.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterRearLeft.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
}
}
void power_Difference(){
float y;
float R;
float L;
if ((channel4 > 1470 & channel4 < 1515) & (channel2 > 1470 & channel2 < 1515)){
thrusterRearRight.writeMicroseconds(1500);
thrusterRearLeft.writeMicroseconds(1500);
thrusterFrontRight.writeMicroseconds(1500);
thrusterFrontLeft.writeMicroseconds(1500);
}
else if ((channel4 > 1470 & channel4 < 1515) & (channel2 < 1470 || channel2 > 1515)) {
thrusterRearRight.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterRearLeft.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterFrontRight.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
thrusterFrontLeft.writeMicroseconds(map(channel2, 988, 2012, 1100, 1900));
}
else if ((channel4 < 1470 || channel4 > 1515) & (channel2 > 1470 & channel2 < 1515)) {
R=map(channel4, 975, 2025, 1900, 1100);
L=map(channel4, 975, 2025, 1100, 1900);
thrusterRearRight.writeMicroseconds(R);
thrusterRearLeft.writeMicroseconds(L);
thrusterFrontRight.writeMicroseconds(R);
thrusterFrontLeft.writeMicroseconds(L);
}
else if ((channel4 < 1470) & (channel2 < 1470 || channel2 > 1515)) {
y = (channel2-(channel2-1500)*(1500-channel4)/525);
R=map(channel2, 975, 2025, 1100, 1900);
L=map(y, 975, 2025, 1100, 1900);
thrusterRearRight.writeMicroseconds(R);
thrusterRearLeft.writeMicroseconds(L);
thrusterFrontRight.writeMicroseconds(R);
thrusterFrontLeft.writeMicroseconds(L);
}
else if ((channel4 > 1515) & (channel2 < 1470 || channel2 > 1515)) {
y = (channel2-(channel2-1500)*(channel4-1500)/525);
R=map(y, 975, 2025, 1100, 1900);
L=map(channel2, 975, 2025, 1100, 1900);
thrusterRearRight.writeMicroseconds(R);
thrusterRearLeft.writeMicroseconds(L);
thrusterFrontRight.writeMicroseconds(R);
thrusterFrontLeft.writeMicroseconds(L);
}
}
void autonomous_Mode(){
}
void loop() {
read_values();
select();
}