The sketch so far with just forward and backward.
#include <Servo.h>
#define number_of_servos 24 // the number of servos, up to 48 for Mega, 12 for other boards
#define servo_pin_start 22
const int battery_led = 13; // select the pin for the battery low LED
const int battery_read_Pin= 10; // input pin for the battery
String status_report="Checking...";
String status_report_sensor="Checking Sensors";
String error_report="None";
int battery_total_return;
int battery_value_ave;
int battery_low=LOW; // set battery variable to battery is charged
int battery_read; // variable to store the value coming from the battery
int battery_sum;
int position_saved[number_of_servos];
int standing_state=LOW;
int i;
int j;
int x;// for multipule reads
int status_number;
int servo_number=number_of_servos;
const int servo_timer_delay=350;
const int low_battery_value=750; //voltage to start low battery state
int number_of_battery_reads=1;
Servo Servos[number_of_servos] ; // max servos is 48 for mega, 12 for other boards
byte forwards[8][number_of_servos]={
{
135 ,90 ,135 ,90 ,45 ,90 ,45 ,90, 180 ,90 ,180 ,90 ,0 ,90 ,0 ,90, 180 ,0 ,180 ,0 ,0 ,180 ,0 ,180 }
,
{
135 ,45 ,135 ,45 ,45 ,90 ,45 ,90, 180 ,180 ,180 ,180 ,0 ,90 ,0 ,90, 180 ,90 ,180 ,90 ,0 ,180 ,0 ,180 }
,
{
135 ,45 ,135 ,45 ,45 ,45 ,45 ,45, 90 ,180 ,90 ,180 ,90 ,90 ,90 ,90, 180 ,180 ,180 ,180 ,0 ,0 ,0 ,0 }
,
{
90 ,135 ,90 ,135 ,90 ,45 ,90 ,45, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0 }
,
{
45 ,135 ,45 ,135 ,0 ,45 ,0 ,45, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0, 0 ,180 ,0 ,180 ,180 ,0 ,180 ,0 }
,
{
45 ,135 ,45 ,135 ,0 ,90 ,0 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 0 ,180 ,0 ,180 ,180 ,0 ,180 ,0 }
,
{
90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90 }
,
{
45 ,45 ,45 ,45 ,135 ,45 ,135 , 45, 180 ,90 ,180 ,90 ,90 ,90 ,90 ,90, 180 ,0 ,180 ,0 ,0 ,180 ,0 ,180 }
};
byte backwards[8][number_of_servos]={
{
45 ,45 ,45 ,45 ,135 ,45 ,135 , 45, 180 ,90 ,180 ,90 ,90 ,90 ,90 ,90, 180 ,0 ,180 ,0 ,0 ,180 ,0 ,180 }
,
{
90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90 }
,
{
45 ,135 ,45 ,135 ,0 ,90 ,0 ,90, 90 ,90 ,90 ,90 ,90 ,90 ,90 ,90, 0 ,180 ,0 ,180 ,180 ,0 ,180 ,0 }
,
{
45 ,135 ,45 ,135 ,0 ,45 ,0 ,45, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0, 0 ,180 ,0 ,180 ,180 ,0 ,180 ,0 }
,
{
90 ,135 ,90 ,135 ,90 ,45 ,90 ,45, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0, 90 ,180 ,90 ,180 ,90 ,0 ,90 ,0 }
,
{
135 ,45 ,135 ,45 ,45 ,90 ,45 ,90, 180 ,180 ,180 ,180 ,0 ,90 ,0 ,90, 180 ,90 ,180 ,90 ,0 ,180 ,0 ,180 }
,
{
135 ,45 ,135 ,45 ,45 ,90 ,45 ,90, 180 ,180 ,180 ,180 ,0 ,90 ,0 ,90, 180 ,90 ,180 ,90 ,0 ,180 ,0 ,180 }
,
{
135 ,90 ,135 ,90 ,45 ,90 ,45 ,90, 180 ,90 ,180 ,90 ,0 ,90 ,0 ,90, 180 ,0 ,180 ,0 ,0 ,180 ,0 ,180 }
};
void setup() {
Serial.begin(115200);
for(servo_number=0; servo_number < number_of_servos; servo_number++){
Servos[servo_number].attach(servo_pin_start+servo_number);
Servos[servo_number].write(forwards[1][servo_number]);
position_saved[servo_number]=(forwards[1][servo_number]);
}
pinMode(battery_led, OUTPUT); // declare the ledPin as an OUTPUT:
}
void battery(){
battery_total_return=0;
battery_sum=0;
for (x=0;x<number_of_battery_reads;x++){
battery_read=analogRead(battery_read_Pin); // read the value from the sensor:
battery_sum= battery_sum + battery_read;
}
battery_total_return = battery_sum / number_of_battery_reads;
if (battery_total_return<low_battery_value){
battery_low=HIGH;
error_report="Battery";
digitalWrite(battery_led, HIGH); // turn the battery low led on
}
else {
battery_low=LOW;
error_report="None";
digitalWrite(battery_led, LOW); // turn the battery low led off:
}
}
void hold(){
for (servo_number=0;servo_number<number_of_servos;servo_number++){
status_report="Holding...";
status_number=0;
Servos[servo_number].write(position_saved[servo_number]); // puts servo in last standing position
position_saved[servo_number]=(position_saved[servo_number]);
}
check_status();
return;
}
void forward() {
for (i=0;i<8;i++){
status_report="Forward";
status_number=i;
for (servo_number=0;servo_number<number_of_servos;servo_number++){
Servos[servo_number].write(forwards[i] [servo_number]); // puts servo in stand position
position_saved[servo_number]=(forwards[i] [servo_number]);
}
check_status();
};
return;
}
void backward() {
for (i=0;i<8;i++){
status_report="Backward";
status_number=i;
for (servo_number=0;servo_number<number_of_servos;servo_number++){
Servos[servo_number].write(backwards[i][servo_number]); // puts servo in stand position
position_saved[servo_number]=(backwards[i][servo_number]);
}
check_status();
}
;
return;
}
void counter_clockwise() {
for (servo_number=0;servo_number<number_of_servos;servo_number++){
Servos[servo_number].write(forwards[i] [servo_number]); // puts servo in stand position
position_saved[servo_number]=(forwards[i] [servo_number]);
}
check_status();
return;
}
void clockwise() {
for (servo_number=0;servo_number<number_of_servos;servo_number++){
Servos[servo_number].write(forwards[i][servo_number]); // puts servo in stand position
position_saved[servo_number]=(forwards[i][servo_number]);
}
check_status();
return;
}
void check_status(){
battery();
debuger();
delay(servo_timer_delay);
if (battery_low==HIGH){
hold();
}
}
void debuger(){
Serial.print(status_report);
Serial.print(" ");
Serial.print(status_number+1);
Serial.write(9);
Serial.print("Error: ");
Serial.println(error_report);
Serial.println(" ");
Serial.print("Leg#");
Serial.write(9);
Serial.print("1");
Serial.write(9);
Serial.print("2");
Serial.write(9);
Serial.print("3");
Serial.write(9);
Serial.print("4");
Serial.write(9);
Serial.print("5");
Serial.write(9);
Serial.print("6");
Serial.write(9);
Serial.print("7");
Serial.write(9);
Serial.println("8");
Serial.println(" ");
Serial.print("Knee ");
Serial.write(9);
Serial.print(position_saved[0]);
Serial.write(9);
Serial.print(position_saved[1]);
Serial.write(9);
Serial.print(position_saved[2]);
Serial.write(9);
Serial.print(position_saved[3]);
Serial.write(9);
Serial.print(position_saved[4]);
Serial.write(9);
Serial.print(position_saved[5]);
Serial.write(9);
Serial.print(position_saved[6]);
Serial.write(9);
Serial.println(position_saved[7]);
Serial.print("Leg ");
Serial.write(9);
Serial.print(position_saved[8]);
Serial.write(9);
Serial.print(position_saved[9]);
Serial.write(9);
Serial.print(position_saved[10]);
Serial.write(9);
Serial.print(position_saved[11]);
Serial.write(9);
Serial.print(position_saved[12]);
Serial.write(9);
Serial.print(position_saved[13]);
Serial.write(9);
Serial.print(position_saved[14]);
Serial.write(9);
Serial.println(position_saved[15]);
Serial.print("Hip ");
Serial.write(9);
Serial.print(position_saved[16]);
Serial.write(9);
Serial.print(position_saved[17]);
Serial.write(9);
Serial.print(position_saved[18]);
Serial.write(9);
Serial.print(position_saved[19]);
Serial.write(9);
Serial.print(position_saved[20]);
Serial.write(9);
Serial.print(position_saved[21]);
Serial.write(9);
Serial.print(position_saved[22]);
Serial.write(9);
Serial.println(position_saved[23]);
Serial.println(" ");
Serial.print("Volt");
Serial.write(9);
Serial.println("Batled");
Serial.print(battery_sum);
Serial.write(9);
Serial.println(battery_low);
Serial.println("----------------------------------------");
Serial.println(" ");
Serial.println(" ");
Serial.println(" ");
Serial.println(" ");
Serial.println(" ");
Serial.println("----------------------------------------");
}
void loop()
{
backward();
forward();
}