Help me out please, robot arm is programmed, but servos just jitter and lock up

So, I’ve programmed my robot arm, I have 6 x 2 actuator controller, as marked in the code “rows and columns”. I have a 180 degree servo as the base turn servo, two 210 degree servos as the base elbow servos, and a servo for each of the other elbow joints and the arm.
My problem is that whenever I power up my robot, all servos rotate to either their 0 or 180 positions, and lock up. I’m thinking it’s an initialization or a programming error.
Here’s the first portion of the code (sorry, it exceeds the 9000 character limit

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include "Keypad.h"
#include <LiquidCrystal_I2C.h>

// fix definierte Servostellungen
#define SERVOMIN  99 // this is the 'minimum' pulse length count (out of 4096) - tested  MG 995 = 95 absolute HW min
#define SERVOMAX  455 // this is the 'maximum' pulse length count (out of 4096) - tested MG 995 = 460 absolute HW max

#define SERVODEF 90 // servo default startposition 
#define SERVO2DEF 90
#define SERVO4DEF 90
#define SERVO5DEF 90

//MIN MAX positions 
//open
#define S5MIN 0
//close
#define S5MAX 150

#define S4MIN 0
#define S4MAX 180
#define S3MIN 0
#define S3MAX 180
#define S2MIN 0
//#define S2MAX 180
#define S2MAX 180
#define S1MIN 30
#define S1MAX 150
#define S0MIN 0
#define S0MAX 180




// Controlboard buttondefninition
const byte ROWS = 2; 
const byte COLS = 6; 

char keys[ROWS][COLS] =
 {{'A','B','C','D','E','F'},
  {'G','H','I','J','K','L'}};
  //ASCII
  //65,66,67,68,69,70
  //71,72,73,74,75,76
  //77,78,79,80,81,82

byte rowPins[ROWS] = {9,8}; 
byte colPins[COLS] = {7,6,5,4,3,2}; 

// reset all buttons
int keyAstate = 0;
int keyBstate = 0;
int keyCstate = 0;
int keyDstate = 0;
int keyEstate = 0;
int keyFstate = 0;
int keyGstate = 0;
int keyHstate = 0;
int keyIstate = 0;
int keyJstate = 0;
int keyKstate = 0;
int keyLstate = 0;

// all servos to startposition
int curr0Pos = SERVODEF;
int curr1Pos = SERVODEF;
int curr2Pos = SERVO2DEF;
int curr3Pos = SERVODEF;
int curr4Pos = SERVO4DEF;
int curr5Pos = SERVO5DEF;

//speed Switch
int speedSwitch = 0; //0=defaulthighspeed, 1=lowspeed
int speedSwitchPin = 19;

int posingDelay = 200;


Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
LiquidCrystal_I2C lcd(0x27);//,20,4);



void setup() {
  // print on display
  // lcd.init();
  lcd.begin(16,2); 
  lcd.clear(); 
  lcd.backlight(); 
  lcd.print("RoboarmControl V0.3");
  lcd.setCursor(0,1);
  lcd.print("booting...");
  delay(100);
     
  //serial Debug
  //Serial.begin(9600);
  //Serial.println("Servo test");

  pwm.begin();
  pwm.setPWMFreq(50);  // workingfreq MG995 50Hz
  
  keypad.addEventListener(keypadEvent); //Event Listener for keyboard
  keypad.setHoldTime(70);               // faster reaction by buttonpressing is 1000mS
  keypad.setDebounceTime(50);           // buttondebaunce  Default is 50mS
  
  pinMode(speedSwitchPin,INPUT);  //speedSwitch Button
  
  // Pin D19 = int4             
  lcd.print("Servos to Zero...");

  pwm.setPWM(1, 0, SERVODEF);
  pwm.setPWM(2, 0, SERVO2DEF);
  pwm.setPWM(3, 0, SERVODEF);
  pwm.setPWM(4, 0, SERVO4DEF);
  pwm.setPWM(5, 0, SERVO5DEF);
  pwm.setPWM(0, 0, SERVODEF); //turn pbase last because of centrifugal force
  // Posing at start
  lcd.setCursor(0,3);
  lcd.print("Posing :)");
 
  pwm.setPWM(5, 0, S5MIN);
  delay(posingDelay);
  pwm.setPWM(4, 0, S4MIN);
  delay(posingDelay);
  pwm.setPWM(3, 0, S3MAX);
  delay(posingDelay);
  pwm.setPWM(2, 0, S2MAX);
  delay(posingDelay); 
  pwm.setPWM(1, 0, 415);
  delay(posingDelay);
  pwm.setPWM(0, 0, S0MAX);
  delay(posingDelay);
  delay(posingDelay);
  pwm.setPWM(0, 0, S0MIN);
  delay(posingDelay);
  delay(posingDelay);
  
  pwm.setPWM(5, 0, SERVO5DEF);
  delay(posingDelay);
  pwm.setPWM(4, 0, SERVO4DEF);
  delay(posingDelay);
  pwm.setPWM(3, 0, SERVODEF);
  delay(posingDelay);
  pwm.setPWM(2, 0, SERVO2DEF);
  delay(posingDelay); 
  pwm.setPWM(1, 0, SERVODEF);
  delay(posingDelay);
  pwm.setPWM(0, 0, SERVODEF);
  delay(posingDelay);
  pwm.setPWM(0, 0, SERVODEF);
  delay(posingDelay);
  
  pwm.setPWM(5, 0, S5MIN);
  delay(50);
  pwm.setPWM(5, 0, S5MAX);
  delay(50);    
  pwm.setPWM(5, 0, S5MIN);
  delay(50);
  pwm.setPWM(5, 0, S5MAX);
  delay(50);
  // end posing
  
  pwm.setPWM(4, 0, 99);
  delay(50);
  pwm.setPWM(5, 0, 280); //open
  delay(50);
   
  curr4Pos = 99;
  curr5Pos = 280;
  lcd.print(" Ready");
  delay(1500);
  lcd.clear();
}

void loop() {
    char key = keypad.getKey();
    
    //DEBUG Servo position INFO
       
    lcd.setCursor(0,0);
    lcd.print("Current Position");
    
    lcd.setCursor(0,1);
    lcd.print("0:");
    lcd.print(curr0Pos);
    lcd.print(" ");
    lcd.setCursor(5,1);
    lcd.print(" 1:");
    lcd.print(curr1Pos);
    lcd.print(" ");
    lcd.setCursor(11,1);
    lcd.print(" 2:");
    lcd.print(curr2Pos);
    lcd.print(" ");
        
    lcd.setCursor(0,2);
    lcd.print("3:");
    lcd.print(curr3Pos);
    lcd.print(" ");
    lcd.setCursor(5,2);
    lcd.print(" 4:");
    lcd.print(curr4Pos);
    lcd.print(" ");
    lcd.setCursor(11,2);
    lcd.print(" 5:");
    lcd.print(curr5Pos);
    lcd.print(" ");
    
    lcd.setCursor(0,3);
    if (speedSwitch == 0){
      lcd.print("Speed: high");
    }
    else{
      lcd.print("Speed: low ");
    }
    
    //debug
    //Serial.println("currPos:");
    //Serial.println(curr0Pos);
    //Serial.println(curr1Pos);
    //Serial.println(curr2Pos);
    //Serial.println(curr3Pos);
    //Serial.println(curr4Pos);
    //Serial.println(curr5Pos);
    //Serial.println(" ");

    
    //calculating eventhandler infos
    if(keyAstate == 2){
      curr0Pos = moveServo(0,0,curr0Pos);
    }
    if(keyBstate == 2){
      curr1Pos = moveServo(1,1,curr1Pos);
    }
    if(keyCstate == 2){
      curr2Pos = moveServo(2,0,curr2Pos);
    }
    if(keyDstate == 2){
      curr3Pos = moveServo(3,1,curr3Pos);
    }
    if(keyEstate == 2){
      curr4Pos = moveServo(4,0,curr4Pos);
    }
    if(keyFstate == 2){
      curr5Pos = moveServo(5,1,curr5Pos);
    }
    if (keyGstate == 2){
      curr0Pos = moveServo(0,1,curr0Pos);
    }
    if (keyHstate == 2){
      curr1Pos = moveServo(1,0,curr1Pos);
    }
    if (keyIstate == 2){
      curr2Pos = moveServo(2,1,curr2Pos);
    }
    if (keyJstate == 2){
      curr3Pos = moveServo(3,0,curr3Pos);
    }
    if (keyKstate == 2){
      curr4Pos = moveServo(4,1,curr4Pos);
    }
    if (keyLstate == 2){
      curr5Pos = moveServo(5,0,curr5Pos);
    }
}

//moves the servos
int moveServo(int servoNr, int servoDirection, int currPos){
 //servospeed
  int currSpeed;
  int servoMax = SERVOMAX;
  int servoMin = SERVOMIN;
  int stepDelay;
  
  if (speedSwitch == 1){
    stepDelay = 10;
    switch (servoNr) {
    case 0:
      currSpeed = 1;
      servoMax = S0MAX;
      servoMin = S0MIN;
    break;
    case 1:
      currSpeed = 2;
      servoMax = S1MAX;
      servoMin = S1MIN;
    break;
    case 2:
      currSpeed = 3;
      servoMax = S2MAX;
      servoMin = S2MIN;
    break;
    case 3:
      currSpeed = 3;
      servoMax = S3MAX;
      servoMin = S3MIN;
    break;
    case 4:
      currSpeed = 10;
      servoMax = S4MAX;
      servoMin = S4MIN;
    break;
    case 5:
      currSpeed = 10;
      servoMax = S5MAX;
      servoMin = S5MIN;
    break;
    }
  }
  else{
    stepDelay = 0;
    switch (servoNr) {
    case 0:
      currSpeed = 20;
      servoMax = S0MAX;
      servoMin = S0MIN;
    break;
    case 1:
      currSpeed = 10;
      servoMax = S1MAX;
      servoMin = S1MIN;
    break;
    case 2:
      currSpeed = 15;
      servoMax = S2MAX;
      servoMin = S2MIN;
    break;
    case 3:
      currSpeed = 25;
      servoMax = S3MAX;
      servoMin = S3MIN;
    break;
    case 4:
      currSpeed = 30;
      servoMax = S4MAX;
      servoMin = S4MIN;
    break;
    case 5:
      currSpeed = 35;
      servoMax = S5MAX;
      servoMin = S5MIN;
    break;
    }
  }

Here’s the remainder of the code:
}
if (servoDirection == 1){
currPos = currPos + currSpeed;
if (currPos > servoMax){currPos = servoMax;}
}
if (servoDirection == 0){
currPos = currPos - currSpeed;
if (currPos < servoMin){currPos = servoMin;}
}

pwm.setPWM(servoNr, 0, currPos); //set position
delay(stepDelay);
return currPos;
}

// Eventhandler for controlboard
void keypadEvent(KeypadEvent key){
switch (keypad.getState()){
case PRESSED:
switch (key){
case ‘A’: //Serial.println(“A pressed”);
// keyAstate = 1;
// do nothing here
break;
break;
}
break;

case RELEASED:
switch (key){
case ‘A’: //Serial.println(“A released”);
keyAstate = 3;
break;
case ‘B’:
keyBstate = 3;
break;
case ‘C’:
keyCstate = 3;
break;
case ‘D’:
keyDstate = 3;
break;
case ‘E’:
keyEstate = 3;
break;
case ‘F’:
keyFstate = 3;
break;
case ‘G’:
keyGstate = 3;
break;
case ‘H’:
keyHstate = 3;
break;
case ‘I’:
keyIstate = 3;
break;
case ‘J’:
keyJstate = 3;
break;
case ‘K’:
keyKstate = 3;
break;
case ‘L’:
keyLstate = 3;
break;

break;
}
break;

case HOLD:
switch (key){
case ‘A’: //Serial.println(“A hold”);
keyAstate = 2;
break;
case ‘B’: //Serial.println(“B hold”);
keyBstate = 2;
break;
case ‘C’: //Serial.println(“C hold”);
keyCstate = 2;
break;
case ‘D’: //Serial.println(“D hold”);
keyDstate = 2;
break;
case ‘E’: //Serial.println(“E hold”);
keyEstate = 2;
break;
case ‘F’: //Serial.println(“F hold”);
keyFstate = 2;
break;
case ‘G’: //Serial.println(“G hold”);
keyGstate = 2;
break;
case ‘H’: //Serial.println(“H hold”);
keyHstate = 2;
break;
case ‘I’: //Serial.println(“I hold”);
keyIstate = 2;
break;
case ‘J’: //Serial.println(“J hold”);
keyJstate = 2;
break;
case ‘K’: //Serial.println(“K hold”);
keyKstate = 2;
break;
case ‘L’: //Serial.println(“L hold”);
keyLstate = 2;
break;
}
break;
}
}

//setSpeed from interrupt 4 / PinA19
void setSpeed(){
if (speedSwitch == 0)
{
speedSwitch = 1;
}
else{
speedSwitch = 0;
}
}

slick395: My problem is that whenever I power up my robot, all servos rotate to either their 0 or 180 positions, and lock up. I'm thinking it's an initialization or a programming error.

Maybe it's a matter of insufficient power, not accommodating all the servo inrush? What's the situation in that regard?

two 210 degree servos as the base elbow servos

Special servos? Most servo problems are due to an inadequate power supply (do not power servos from the arduino), or poor grounding between the servo and the arduino.

Hi,

Can you please post a copy of your circuit including power supplies, in CAD or a picture of a hand drawn circuit in jpg, png?

If possible a picture of your project. Also the spec or links to spec of your actuator and servo hardware.

We have no idea how much current is needed for your servos and actuators and how you are driving them.

Thanks.. Tom... :)