Ionic Space Thruster Controller

#include <Servo.h>

// https://www.arduino.cc/reference/en/libraries/ezbutton/
#include <ezButton.h>


#define PIN_LEFT   4   // pin of left button
#define PIN_RIGHT  3   // pin of right button


#define PIN_SERVO01 5  // pin of srvo motor #1
#define PIN_SERVO02 6  // pin of srvo motor #2

// assign pin as input for buttons
ezButton buttonLeft(PIN_LEFT);
ezButton buttonRight(PIN_RIGHT);

// create servo objects to control a servo
Servo servo01;
Servo servo02;

bool servoStopped = false;
bool servo01enabled = true;
bool servo02enabled = true;

void setup() {
  Serial.begin(9600);
  Serial.println("-- controller started --");

  // attaches the servo
  servo01.attach(PIN_SERVO01);
  sendCommand(servo01, "stop");
  servo02.attach(PIN_SERVO02);
  sendCommand(servo02, "stop");

  // set debounce time in milliseconds
  buttonLeft.setDebounceTime(50);
  buttonRight.setDebounceTime(50);
}

void loop() {
  if (Serial.available() > 0) {
    String incomingCommand = Serial.readString();
    incomingCommand.trim();
    Serial.print("received cmd: [");
    Serial.print(incomingCommand);
    Serial.println("]");

    // Keyborad controll:   "q a s d e"
    if (incomingCommand == "a") {
      sendDualCommand("left v0");
    } else if (incomingCommand == "q") {
      sendDualCommand("left v1");

    } else if (incomingCommand == "d") {
      sendDualCommand("right v0");
    } else if (incomingCommand == "e") {
      sendDualCommand("right v1");

    } else if (incomingCommand == "s") {
      sendDualCommand("stop");

    } else if (incomingCommand == "1") {
      servo01enabled = ! servo01enabled;
    } else if (incomingCommand == "2") {
      servo02enabled = ! servo02enabled;

    } else {
      sendDualCommand("go " + incomingCommand);
    }
  }

  buttonLeft.loop();
  buttonRight.loop();


  if (buttonLeft.isPressed()) {
    sendDualCommand("left v0");
  } else if (buttonRight.isPressed()) {
    sendDualCommand("right v0");
  }

  if (!servoStopped && (buttonLeft.isReleased() || buttonRight.isReleased())) {
    sendDualCommand("stop");
  }

}

void sendDualCommand(String cmd) {
  if (servo01enabled) {
    sendCommand(servo01, cmd);
  }

  if (servo02enabled) {
    sendCommand(servo02, cmd);
  }
}

String getSernoName(Servo &servo) {
  if (&servo == &servo01) {
    return "servo01";
  } else if (&servo == &servo02) {
    return "servo02";
  } else {
    return "unknonw";
  }
}

#define SERVO_WRITE_LEFT_V2    75
#define SERVO_WRITE_LEFT_V0    84  // Slow
#define SERVO_WRITE_STOP       90 
#define SERVO_WRITE_RIGHT_V0   99  // Slow
#define SERVO_WRITE_RIGHT_V1   105

void sendCommand(Servo &servo, String cmd) {
  Serial.print("  send cmd: ");
  Serial.print(cmd);
  Serial.print(" to ");
  Serial.print(getSernoName(servo));
  Serial.println("");
  Serial.flush();

  servoStopped = false;

  if ((cmd == "left") || (cmd == "left v0")) {
    servo.write(SERVO_WRITE_LEFT_V0);
  } else if (cmd == "left v1") {
    servo.write(SERVO_WRITE_LEFT_V2);
  } else if (cmd == "right" || cmd == "right v0") {
    servo.write(SERVO_WRITE_RIGHT_V0);
  } else if (cmd == "right v1") {
    servo.write(SERVO_WRITE_RIGHT_V1);
  } else if (cmd == "stop") {
    servo.write(SERVO_WRITE_STOP);
    servoStopped = true;


  } else if (cmd.startsWith("go")) {
    servo.write(cmd.substring(3).toInt());

  } else {
    Serial.println("   --- unknonw command ---");
  }
}