Robot arm initialization

Projects Programming
8

When you have bunches of three or more variables that are consecutively numbered you will almost certainly be better off with arrays. It makes the code shorter and easier to understand. By using a defined constant for the size of the array you can add (or remove) servos easily. (Example sketch not tested.)

#include <VarSpeedServo.h>
const int ServoCount = 4;
VarSpeedServo ArmServos[ServoCount];
const int ArmServoPins[ServoCount] = {12, 11, 10, 9};
const int PotPins[ServoCount] = {A0, A1, A2, A3};

int ArmAngles[ServoCount];

const int button1 = 2; //Buttons
const int button2 = 3;

int button1Presses = 0; //Button values
boolean button2Pressed = false;

int ServoPosSaves[ServoCount][5]; //position saves

void GetArmAngles() {
  for (int i = 0; i < ServoCount; i++) {
    ArmAngles[i] = map(analogRead(ArmPins[i]), 0, 1023, 0, 179);
  }
}

void setup() {
  GetArmAngles();

  for (int i = 0; i < ServoCount; i++) {
    ArmServos[i].write(ArmAngle[i], 10)
    ArmServos[i].attach(ArmServoPins[i]);
  }

  pinMode(button1, INPUT);
  pinMode(button2, INPUT);

  Serial.begin(9600);
}

void loop() {
  // Position the arm according to the knob setting
  GetArmAngles();
  for (int i = 0; i < ServoCount; i++) {
    ArmServos[i].write(ArmAngle[i], 10)
  }

  if (digitalRead(button1) == HIGH) { // This will check how many times button1 is pressed and save the positions to an array depending on how many times it is pressed; switch/case works like a if statement
    button1Presses++;
    switch (button1Presses) {
      case 1:
      case 2:
      case 3:
      case 4:
      case 5:
        for (int i = 0; i < ServoCount; i++) {
          ServoPosSaves[i][buttonPresses - 1] = ArmAngle[i];
        }
        Serial.print("Pos ");
        Serial.print(buttonPresses);
        Serial.println(" Saved");
        break;
    }
  }

  if (digitalRead(button2) == HIGH) { // Pretty self-explnatory here
    button2Pressed = true;
  }

  if (button2Pressed) { // if the boolean button2Press is true, then the servos will run though all their saved positions
    for (int s = 0; s < 5; s++) {
      for (int i = 0; i < ServoCount; i++) {
        ArmServos[i].write(ServoPosSaves[i][s]);
      }
      Serial.println(" potentimeter Angles: ");
      for (int i = 0; i < ServoCount; i++) {
        Serial.println(ServoPosSaves[i][s]);
      }

      delay(1000);
    }
  }
  delay(100);
}