We are using an Arduino Mega 2560 to control four T200 BlueRobotics thrusters with ESCs. We read input from a Playstation Dual Shock controller using a separate python script, and then communicate to the Arduino via the Serial port. Our python code is here:
# operation make the controller work
import pygame
import time
import json
import pygame
import serial
import serial.tools.list_ports
servo_b_open = 60
servo_b_close = 180
servo_a_open = 60
servo_a_close = 180
# initialize serial monitor
correctdevice = str()
for port in serial.tools.list_ports.comports():
# print(port.__dict__)
if "arduino" in port.manufacturer.lower():
#if "usb" in port.device.lower():
correctdevice = port.device
print("Using serial port: ", correctdevice)
ser = serial.Serial(correctdevice, 19200, timeout=0.005)
def writeToSerial(msg):
print("writing", msg.encode())
ser.write(msg.encode())
def translate(value, leftMin, leftMax, rightMin, rightMax):
# Figure out how 'wide' each range is
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
# Convert the left range into a 0-1 range (float)
valueScaled = float(value - leftMin) / float(leftSpan)
# Convert the 0-1 range into a value in the right range.
return rightMin + (valueScaled * rightSpan)
# controller testing
num = 0
pygame.init()
pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
print(joystick_count)
controller = pygame.joystick.Joystick(0)
controller.init()
while True:
for event in pygame.event.get():
event_dict = event.dict
if event_dict.get("axis") == 4:
degrees = translate (event.dict.get("value"), -1, 1, servo_b_close, servo_b_open)
print(degrees)
writeToSerial(str(int(degrees)) + "y")
if event_dict.get("axis") == 5:
degrees = translate (event.dict.get("value"), -1, 1,servo_a_close, servo_a_open)
print(degrees)
writeToSerial(str(int(degrees)) + "x")
if event_dict.get("axis") == 0:
degrees = translate (event.dict.get("value"), -1, 1,1400, 1600)
degrees = round(degrees/10)*10
print(degrees)
if degrees > 1490 and degrees < 1510:
writeToSerial(str(int(1500)) + "a")
writeToSerial(str(int(1500)) + "d")
else:
writeToSerial(str(int(degrees)) + "a")
writeToSerial(str(int(degrees)) + "d")
if event_dict.get("axis") == 1:
c_degrees = translate (event.dict.get("value"), -1, 1,1400, 1600)
c_degrees = round(c_degrees/10)*10
if c_degrees > 1490 and c_degrees < 1510:
writeToSerial(str(int(1500)) + "c")
else:
writeToSerial(str(int(c_degrees)) + "c")
b_degrees = translate (event.dict.get("value"), -1, 1,1600, 1400)
b_degrees = round(b_degrees/10)*10
if b_degrees > 1490 and b_degrees < 1510:
writeToSerial(str(int(1500)) + "b")
else:
writeToSerial(str(int(b_degrees)) + "b")
if event_dict.get("axis")== 2 :
a_degrees = translate (event.dict.get("value"), -1, 1,1600, 1400)
a_degrees = round(a_degrees/10)*10
if a_degrees > 1490 and a_degrees < 1510:
writeToSerial(str(int(1500)) + "a")
else:
writeToSerial(str(int(a_degrees)) + "a")
d_degrees = translate (event.dict.get("value"), -1, 1,1400, 1600)
d_degrees = round(d_degrees/10)*10
if d_degrees > 1490 and d_degrees < 1510:
writeToSerial(str(int(1500)) + "d")
else:
writeToSerial(str(int(d_degrees)) + "d")
if event_dict.get("axis")== 3 :
b_degrees = translate (event.dict.get("value"), -1, 1,1600, 1400)
b_degrees = round(b_degrees/10)*10
if b_degrees > 1490 and b_degrees < 1510:
writeToSerial(str(int(1500)) + "b")
else:
writeToSerial(str(int(b_degrees)) + "b")
c_degrees = translate (event.dict.get("value"), -1, 1,1400, 1600)
c_degrees = round(c_degrees/10)*10
if c_degrees > 1490 and c_degrees < 1510:
writeToSerial(str(int(1500)) + "c")
else:
writeToSerial(str(int(c_degrees)) + "c")
pygame.time.wait(15)
out = ser.readline()
if out:
print(out.decode(), end = '')
and our Arduino code is here:
#include <Servo.h>
#define THRUSTERS 4 // the number of Thrusters
#define STOP 1500
#define SERVOS 4 // the number of Servos
int thrusterPins[THRUSTERS] = {2, 4, 5, 3}; // Thrusters on pins 2 to 5
Servo thrusters[THRUSTERS];
int servoPins[SERVOS] = {8, 9, 11, 10}; // Servos on pins 6 to 9 (w and x are camera; y and z are claws)
Servo servos[SERVOS];
int servoPositions[SERVOS] = {10,10, 0, 0};
#define MAX_THRUSTER_STEP 5
int lastThrusterSpeeds[THRUSTERS] = {1500, 1500, 1500, 1500};
int desiredThrusterSpeeds[THRUSTERS] {1500, 1500, 1500, 1500};
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
void setup() {
Serial.begin(19200);
for(int i=0; i < THRUSTERS; i++)
{
thrusters[i].attach(thrusterPins[i]);
thrusters[i].writeMicroseconds(STOP);
}
for(int i=0; i < SERVOS; i++)
{
servos[i].attach(servoPins[i], 1000, 2000);
servos[i].write(servoPositions[i]);
}
delay(7000);
// put your setup code here, to run once:
Serial.println("Started!");
}
void loop() {
serviceSerial();
// Go through all the thrusters
for(uint8_t i = 0; i < THRUSTERS; ++i)
{
// Get the speed the pilot set
int speed = desiredThrusterSpeeds[i];
if(speed > STOP)
{
// If it is in a faster forward direction, only ramp up a small step
if(speed > lastThrusterSpeeds[i]) {
speed = MIN(lastThrusterSpeeds[i] + MAX_THRUSTER_STEP, speed);
}
} else if (speed < STOP) {
// If it is in a faster reverse direction, only ramp up a small step
if(speed < lastThrusterSpeeds[i]) {
speed = MAX(lastThrusterSpeeds[i] - MAX_THRUSTER_STEP, speed);
}
}
// If the speed we're ramping to isn't the speed the thruster is
// spinning at, set the thruster to the new speed.
if(speed != lastThrusterSpeeds[i]) {
lastThrusterSpeeds[i] = speed;
thrusters[i].write(speed);
Serial.print("Thruster ");
Serial.print(i);
Serial.print(" ramped to ");
Serial.println(speed);
}
}
delay(15);
// put your main code here, to run repeatedly:
}
void serviceSerial()
{
if (Serial.available())
{
int speed = Serial.parseInt();
char ch = Serial.read();
if (ch >= 'a' && ch < 'a' + THRUSTERS)
{
char thrusterIndex = ch - 'a';
Serial.print("Thruster ");
Serial.print(thrusterIndex + 1);
Serial.print(" set to ");
Serial.println(speed);
desiredThrusterSpeeds[thrusterIndex] = speed;
}
if (ch >= 'w' && ch < 'w' + 2)
{
Serial.print("Servo ");
Serial.print(ch - 'w' + 1);
Serial.print(" add to speed ");
Serial.println(speed);
int currentServoPosition = servoPositions[ch - 'w'];
if (speed >0 && currentServoPosition + speed <= 170) {
servoPositions[ch - 'w'] = speed + currentServoPosition;
servos[ch - 'w'].write(servoPositions[ch - 'w'] );
Serial.println(servoPositions[ch - 'w'] );
}
else if(speed<0 && currentServoPosition + speed >= 10) {
servoPositions[ch - 'w'] = speed + currentServoPosition;
servos[ch - 'w'].write(servoPositions[ch - 'w'] );
Serial.println(servoPositions[ch - 'w'] );
}
}
if (ch >= 'y' && ch < 'y' + 2) {
servoPositions[ch - 'y' + 2] = speed;
servos[ch - 'y' + 2].write(servoPositions[ch - 'y' + 2] );
}
}
}
With the current code, we should be setting speeds between 1400 and 1600. However, we are getting some output that is setting it to speeds that are negative or very large and outside that range. The thrusters are connecting and disconnecting, and have erratic behavior.
Has anyone successfully controlled their thrusters this way? Would you be willing to take a look at our code and see if you see anything problematic? Thank you!