thruster.ino

#include <Servo.h>

Servo left_servo, right_servo;

int ESC_PIN_1 = 8;
int ESC_PIN_2 = 10;

int HIGH_SPEED = 1600;
int LOW_SPEED = 1400;
int STOP_SPEED = 1500;

int DELAY = 1000;


int set_speed(int right_speed, int left_speed) {
  left_servo.writeMicroseconds(left_speed);
  right_servo.writeMicroseconds(right_speed);
  delay(DELAY);
  return 0;
}


void setup() {
  Serial.begin(9600);
  left_servo.attach(ESC_PIN_1);
  right_servo.attach(ESC_PIN_2);
  left_servo.writeMicroseconds(STOP_SPEED);
  delay(DELAY);
  right_servo.writeMicroseconds(STOP_SPEED);
  delay(DELAY);
}


void loop() {

  int cmd_signal = Serial.read();
  switch (cmd_signal) {
    // Sppeds are centered around 1500, but due to some mistake
    // the wires in the ESC's were swapped, so we need to give it
    // the opposite to the desired speed (with 1500 as center).
    case 'w':
      set_speed(HIGH_SPEED, LOW_SPEED);
      break;
    case 's':
      set_speed(LOW_SPEED, HIGH_SPEED);
      break;
    case 'a':
      set_speed(HIGH_SPEED, HIGH_SPEED);
      break;
    case 'd':
      set_speed(LOW_SPEED, LOW_SPEED);
      break;
    default:
      set_speed(STOP_SPEED, STOP_SPEED);
  }
}