Controlling a five servo robot arm with serial monitor

This is simple code controlling a five servo robot arm with the serial monitor. This code works but I'm always looking for ways to improve.

#include <VarSpeedServo.h>

VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo servo5;

int oneValue = 85;
int twoValue = 160;
int threeValue = 180;
int fourValue = 100;
int fiveValue = 150;
int goData = 0;

char receivedChar = 0;

void setup() {
  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);
  servo5.attach(8);
  servo1.write(85, 60);
  servo3.write(110, 60, true);
  servo2.write(160, 60, true);
  servo3.write(180, 60, true);
  servo4.write(100, 60);
  servo5.write(150, 60);
  Serial.begin(9600);
  Serial.println("<Arduino is ready>");
  Serial.print("oneValue");
  Serial.println(oneValue);
  Serial.print("twoValue");
  Serial.println(twoValue);
  Serial.print("threeValue");
  Serial.println(threeValue);
  Serial.print("fourValue");
  Serial.println(fourValue);
  Serial.print("fiveValue");
  Serial.println(fiveValue);
}

void loop() {
  if (Serial.available()) {
    while (Serial.available()) {
      receivedChar = Serial.read();
      Serial.println(receivedChar);
      goData = receivedChar;
      Serial.print("goData");
      Serial.println(goData);
    }
    if (goData == 122) {  //z
      oneValue += 5;
      servo1.write(oneValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("oneValue");
      Serial.println(oneValue);
    }
    if (goData == 120) {  //x
      oneValue -= 5;
      servo1.write(oneValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("oneValue");
      Serial.println(oneValue);
    }
    if (goData == 97) {  //a
      twoValue += 5;
      servo2.write(twoValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("twoValue");
      Serial.println(twoValue);
    }
    if (goData == 115) {  //s
      twoValue -= 5;
      servo2.write(twoValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("twoValue");
      Serial.println(twoValue);
    }
    if (goData == 113) {  //q
      threeValue += 5;
      servo3.write(threeValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("threeValue");
      Serial.println(threeValue);
    }
    if (goData == 119) {  //w
      threeValue -= 5;
      servo3.write(threeValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("threeValue");
      Serial.println(threeValue);
    }
    if (goData == 101) {  //e
      fourValue += 5;
      servo4.write(fourValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("fourValue");
      Serial.println(fourValue);
    }
    if (goData == 114) {  //r
      fourValue -= 5;
      servo4.write(fourValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("fourValue");
      Serial.println(fourValue);
    }
    if (goData == 100) {  //d
      fiveValue += 5;
      servo5.write(fiveValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("fiveValue");
      Serial.println(fiveValue);
    }
    if (goData == 102) {  //f
      fiveValue -= 5;
      servo5.write(fiveValue);
      goData = 0;
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("fiveValue");
      Serial.println(fiveValue);
    }
    if (goData == 99) {  //c
      servo1.write(85, 60);
      servo3.write(110, 60, true);
      servo2.write(160, 60, true);
      servo3.write(180, 60, true);
      servo4.write(100, 60);
      servo5.write(150, 60);
      Serial.print("goData");
      Serial.println(goData);
      Serial.print("oneValue");
      Serial.println(85);
      Serial.print("twoValue");
      Serial.println(160);
      Serial.print("threeValue");
      Serial.println(180);
      Serial.print("fourValue");
      Serial.println(100);
      Serial.print("fiveValue");
      Serial.println(150);
    }
  }
}
1 Like

Reduce redundancy with arrays, for example... creating instances...

for (int i = 0; i < 5; i++) {
  VarSpeedServo servo[i];
}

Reduce redundancy with functions, for example...

char *somethingArray[] = {"somethingText", "somethingElse"};
byte number[] = {7, 8};

void setup() {
  Serial.begin(115200);
  printSomething(somethingArray[0], number[1]);
  printSomething(somethingArray[1], number[0]);
}

void loop() {
  // empty
}

void printSomething(char *wordthing, byte value) {
  Serial.println(wordthing);
  Serial.println(value);
}
2 Likes
#include <VarSpeedServo.h>
const byte Amount = 5;
VarSpeedServo servo[Amount];
const byte Default[Amount] = {85, 160, 180, 100, 150};
byte Value[Amount] = {85, 160, 180, 100, 150};

void setup() {
  Serial.begin(115200);
  Serial.println("<Arduino is ready>");
  for (byte i = 0; i < Amount; i++)servo[i].attach(4 + i);
  GoDefault();
}
void GoDefault() {
  for (byte i = 0; i < Amount; i++) Value[i] = Default[i];

  servo[0].write(Value[0], 60);
  servo[2].write(110, 60, true);
  servo[1].write(Value[1], 60, true);
  servo[2].write(Value[2], 60, true);
  servo[3].write(Value[3], 60);
  servo[4].write(Value[4], 60);

  PrintArray();
}

void PrintArray() {
  for (byte i = 0; i < Amount; i++) {
    Serial.print(Value[i]);
    Serial.print('\t');
  }
  Serial.println();
  Serial.flush();
}

void loop() {
  if (Serial.available()) {
    char goData = Serial.read();
    if (goData < ' ' || goData > 127)return;
    if (goData == 'z')oneValue += 5;
    if (goData == 'x') oneValue -= 5;
    if (goData == 'a') twoValue += 5;
    if (goData == 's')twoValue -= 5;
    if (goData == 'q')threeValue += 5;
    if (goData == 'w')threeValue -= 5;
    if (goData == 'e')fourValue += 5;
    if (goData == 'r')fourValue -= 5;
    if (goData == 'd')fiveValue += 5;
    if (goData == 'f') fiveValue -= 5;
    
    if (goData == 'c') GoDefault();
    else for (byte i = 0; i < Amount; i++) servo[i].write(Value[i]);
    
    if (goData == 'v') PrintArray();
  }
}

I've found it easier to use a lower case for reduction, an upper case for increase. It's sort of natural, shift increases, plain decreases.

i preffer protocol:first digit is number of servo, next numbers until not digit separator is a value. looks like:

185;2160 3180,4100:5150"

How does one lead with servo #16, if you don't use leading zeros? Would seem to me to be open to interpretation.
Neither of us is wrong, by the way, there are many ways to do this successfully. In the specific case of throwing servos between two states, I find single character, easily remembered protocols to be worthy. YMMV.