Hey guys,
I have been working on programming my 4 T100 thrusters to control forward/reverse and vertical movement of my ROV. I think that my code in running along the correct direction so far but I am unsure if it is completely right. Would you mind giving me some pointers and advice on how to proceed if my code is incorrect? I am posting it below:
#include <Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
const int servoRight = 8; //initializing servo pins
const int servoLeft = 7;
const int servoUP = 6;
const int servoDOWN = 5;
void setup() {
Serial.begin(9600); //turn serial on
servo1.attach(8); //set servo1 pin 8
servo2.attach(7); //set servo2 pin 7
servo3.attach(6); //set servo3 pin 6
Servo4.attach(5); //set servo4 pin 5
// send “stop” signal to ESC.
servo1.writeMicroseconds(1500);
servo2.writeMicroseconds(1500);
servo3.writeMicroseconds(1500);
servo4.writeMicroseconds(1500);
delay(5000); // 5 sec delay to allow the ESC to //recognize the stopped signal
}
void loop() {
if (Serial.available()) {
// Turning Left, input of TL in serial monitor means Turn Left
char a = Serial.read(); //stores first value of the serial input as a character
char b = Serial.read(); //checks if same as necessary input
char c = Serial.read(); // for int values only
char d = Serial.read(); //for int values only
Serial.write(a);
Serial.write(b);
Serial.write(c);
Serial.write(d);
if (a == ‘T’) {
if (b == ‘L’) {
servo1.writeMicroseconds(1700); // then gives commands
servo2.writeMicroseconds(1500);
Serial.write(“TL”);
}
// Turning Right
if (b == ‘R’) {
servo1.writeMicroseconds(1500);
servo2.writeMicroseconds(1700);
Serial.write(“TR”);
}
}
// Forward and Reverse
else {
if (a == ‘1’) {
if (b == ‘5’) {
servo1.writeMicroseconds(1500); //don’t move
servo2.writeMicroseconds(1500);
}
else if (b == ‘6’) {
servo1.writeMicroseconds(1600); //slow forward
servo2.writeMicroseconds(1600);
}
else if (b == ‘7’) {
servo1.writeMicroseconds(1700); //faster forward
servo2.writeMicroseconds(1700);
}
else if (b == ‘4’) {
servo1.writeMicroseconds(1400); //slow reverse
servo2.writeMicroseconds(1400);
}
else if (b == ‘3’) {
servo1.writeMicroseconds(1300); //faster reverse
servo2.writeMicroseconds(1300);
}
}
}
}
// Up and Down (Vertical Movement)
else {
if (a == ‘1’) {
if (b == ‘5’) {
servo3.writeMicroseconds(1500);
servo4.writeMicroseconds(1500);
}
else if (b == ‘6’) {
servo3.writeMicroseconds(1600);
servo4.writeMicroseconds(1600);
}
else if (b == ‘7’) {
servo3.writeMicroseconds(1700);
servo4.writeMicroseconds(1700);
}
else if (b == ‘4’) {
servo3.writeMicroseconds(1400);
servo4.writeMicroseconds(1400);
}
else if (b == ‘3’) {
servo3.writeMicroseconds(1300);
servo4.writeMicroseconds(1300);
}
}
}
}
else {
servo1.writeMicroseconds(1600); // if no forward command
servo2.writeMicroseconds(1600); // then just go straight
servo3.writeMicroseconds(1500); // if no vertical command
servo4.writeMicroseconds(1500); // stay level
}
}