Servo Twitching on Bipedal Robot

[dennoid]

I am new to Arduino and I am trying to make 6 DOF Bipedal robot. I have connected servos to data pins on the Arduino, and the power is supplied from an external power source. I have tried a 5-7V 2A power supply and a 7.4V lipo battery capable of 10+ amps, I still get the same result, which is servos twitching uncontrollably. I have tried to put a 150ms delay on the servo attach so they don't all activate at once, but this doesn't work either.

#define hipLOffset 105
#define kneeLOffset 155
#define ankleLOffset 85
#define hipROffset 82
#define kneeROffset 25
#define ankleROffset 85

#define l1 5
#define l2 5.7

#define stepClearance 1
#define stepHeight 10

#include <Servo.h>
#include "constants.h"

Servo hipL;
Servo hipR;
Servo kneeL;
Servo kneeR;
Servo ankleL;
Servo ankleR;

void updateServoPos(int target1, int target2, int target3, char leg){
  if (leg == 'l'){
    hipL.write(hipLOffset - target1); 
    kneeL.write(kneeLOffset - target2);
    ankleL.write(2*ankleLOffset - target3);
  }
  else if (leg == 'r'){
    hipR.write(hipROffset + target1);
    kneeR.write(kneeROffset + target2);
    ankleR.write(target3);
  }
}

void pos(float x, float z, char leg){
  float hipRad2 = atan(x/z);
  float hipDeg2 = hipRad2 * (180/PI);

  float z2 = z/cos(hipRad2);

  float hipRad1 = acos((sq(l1) + sq(z2) - sq(l2))/(2*l1*z2));
  float hipDeg1 = hipRad1 * (180/PI);
  
  float kneeRad = PI - acos((sq(l1) + sq(l2) - sq(z2))/(2*l1*l2));

  float ankleRad = PI/2 + hipRad2 - acos((sq(l2) + sq(z2) - sq(l1))/(2*l2*z2));
  
  float hipDeg = hipDeg1 + hipDeg2;
  float kneeDeg = kneeRad * (180/PI);
  float ankleDeg = ankleRad * (180/PI);

//  Serial.print(hipDeg);
//  Serial.print("\t");
//  Serial.print(kneeDeg);
//  Serial.print("\t");
//  Serial.println(ankleDeg);

  updateServoPos(hipDeg, kneeDeg, ankleDeg, leg);  
}

void takeStep(float stepLength, int stepVelocity){
  for (float i = stepLength; i >= -stepLength; i-=0.5){
    pos(i, stepHeight, 'r');
    pos(-i, stepHeight - stepClearance, 'l');
    delay(stepVelocity);
  }

  for (float i = stepLength; i >= -stepLength; i-=0.5){
    pos(-i, stepHeight - stepClearance, 'r');
    pos(i, stepHeight, 'l');
    delay(stepVelocity);
  }
}

void initialize(){
  for (float i = 10.7; i >= stepHeight; i-=0.1){
    pos(0, i, 'l');
    pos(0, i, 'r');
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  delay(150);
  hipL.attach(9);
  delay(150);
  hipR.attach(8);
  delay(150);
  kneeL.attach(7);
  delay(150);
  kneeR.attach(6);
  delay(150);
  ankleL.attach(5);
  delay(150);
  ankleR.attach(4);
  delay(150);
  hipL.write(hipLOffset);
  kneeL.write(kneeLOffset);
  ankleL.write(ankleLOffset);
  
  hipR.write(hipROffset);
  kneeR.write(kneeROffset);
  ankleR.write(ankleROffset);

  delay(5000);
  
  initialize();
}

void loop() {
  // put your main code here, to run repeatedly:
  takeStep(2, 0);
}

Circuit picture shown

[Railroader]

You have likely spent a lot of time getting this far. I ask You if You could use some time producing a wiring diagram, schematics and post it.

[dennoid]

I have posted the circuit, I could only add one picture

[Railroader]

Sorry but that foto is useless. It's like looking down into a jungle.
Use pen and paper unless You feel comfortable with some simple 2D cad program.

[dennoid]

[Railroader]

Thanks. You tried but You posted a hand made picture, not a drawing.
It doesn't help me.

[jremington]

I have tried a 5-7V 2A power supply

Way too little current capacity. Uncontrollable twitching is almost always a sign of problematic power supply or wiring. Budget 1 Ampere per servo for small servos, like the SG-90, or 3 Amperes/servo for large servos, like the MG996R. Be sure to connect the grounds.

Best to use a servo power distribution board, like this.

Please post a link to the servos you are actually using. Most are not rated for 7.4V.