Wolf: Search and Rescue Multi-Terrain Robot

#include

const int ServoNum = 12;

Servo legServo[ServoNum];
int servoPos[ServoNum];
int servoIndex;

int servoPin[ServoNum] = {23, 24, 25,
26, 27, 28,
29, 30, 31,
32, 33, 34};

void setup() {

Serial.begin(9600);
servoIndex = 0;

// ================ Servos ==================
for (int i=0; i<ServoNum; i++){
legServo[i].attach(servoPin[i]);
delay(50);
legServo[i].writeMicroseconds(1500);
servoPos[i] = 1500;
delay(100);
}
delay(500);

Serial.println(“Ready”);
Serial.println(“enter + to increment”);
Serial.println(“enter – to decrement”);
Serial.println(“enter n to proceed to next servo”);
Serial.println(“enter l to go back to last servo”);

}

void loop(){

if ( Serial.available()) {
char ch = Serial.read();

switch(ch){
case ‘+’:
servoPos[servoIndex] += 3;
if(servoPos[servoIndex] >= 2350){
servoIndex = 2350;
Serial.println(“You can’t turn it up anymore (you might damage it!”);
}
else{
legServo[servoIndex].write(servoPos[servoIndex]);
Serial.println(servoPos[servoIndex]);
delay(100);
}
break;

case ‘-‘:
servoPos[servoIndex] -= 3;
if(servoPos[servoIndex] <= 650){
servoIndex = 650;
Serial.println(“You can’t turn it down anymore (you might damage it!”);
}
else{
legServo[servoIndex].write(servoPos[servoIndex]);
Serial.println(servoPos[servoIndex]);
delay(100);
}
break;

case ‘n’:
if(++servoIndex >= 12){
servoIndex = 11;
Serial.println(“we have reached last servo”);
}
else{
Serial.print(“Switched to Pin “);
Serial.println(servoPin[servoIndex]);
}
break;

case ‘l’:
if(–servoIndex < 0){
servoIndex = 0;
Serial.println(“we have reached first servo”);
}
else{
Serial.print(“Switched to Pin “);
Serial.println(servoPin[servoIndex]);
}
break;

default:
Serial.println(“Unknown Command… “);

}

coor GaitCalculate (){
//Calculate Gait positions

for (int LegIndex = 0; LegIndex < 4; LegIndex++){

if (GaitStep == GaitLegNr[LegIndex]) {
GaitPos[LegIndex].Y = -LegLiftHeight/2;
}
else if (GaitStep == GaitLegNr[LegIndex]+1) {
GaitPos[LegIndex].Z = 0;
GaitPos[LegIndex].Y = -LegLiftHeight;
}
else if (GaitStep == GaitLegNr[LegIndex]+2) {
GaitPos[LegIndex].Z = WalkLength/2;
GaitPos[LegIndex].Y = -LegLiftHeight/2;
}
else if (GaitStep == GaitLegNr[LegIndex]+3) {
GaitPos[LegIndex].Y = 0;
}
else{
// move body forward
GaitPos[LegIndex].Z -= WalkLength/TLDivFactor;

}

}

//Advance to the next step
if (++GaitStep > StepsInGait)
GaitStep = 1;
}