void loop() {

//STARTING POSITION
Set_Leg_Position_0Cartes(‘A’, 50,10,down);
Set_Leg_Position_0Cartes(‘B’, 50, -10,down);
Set_Leg_Position_0Cartes(‘C’, -50,50,down);
Set_Leg_Position_0Cartes(‘D’, -50, -50,down);
//STEP 1
Step_in_Y(‘B’, -10,110);

//SHIFT1
Move_in_Y(‘A’, 10,-50);
Move_in_Y(‘B’, 110,50);
Move_in_Y(‘C’, 50,-10);
Move_in_Y(‘D’, -50,-110);

//STEP 2
Step_in_Y(‘D’, -110,10);

//STEP 3
Step_in_Y(‘C’, -10,110);

//SHIFT1
Move_in_Y(‘A’, -50,-110);
Move_in_Y(‘B’, 50,-10);
Move_in_Y(‘C’, 110,50);
Move_in_Y(‘D’, 10,-50);

//STEP 4
Step_in_Y(‘A’, -110,10);
03/04/2013

[sourcecode language=”cpp”]
//
==================================================
=========
// blog.OscarLiang.net
// Created on 04/04/2013
//
//
==================================================
=========
//
// A very simple program to calibrate multiple servo offsets.
// Especially useful if you have a lot of servos, for example
// Hexapod Robots and Quadruped Robots.
//
//
==================================================
=========
//
// Instructions:
//
// 1. change variable “ServoNum” – number of servos you have.
// 2. change variable “servoPin” – the pins used by your servos.
// 3. upload code to Arduino.
// 4. open serial monitor, use baud rate 9600
// 5. enter + or – in serial monitor to calibrate servo, there
// is a reading printed on the monitor.
// 6. enter n or l to move to next or last servo.
//
//
==================================================
===========

#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… “);

}
}
}
[/sourcecode]

15/04/2013

[sourcecode language=”cpp”]

void GaitSelect (){

//begining of the step cycle for each leg (with reference to the first
leg)
GaitLegNr[LR] = 1;
GaitLegNr[LF] = 6;
GaitLegNr[RR] = 11;
GaitLegNr[RF] = 16;

NrLiftedPos = 5; // number of steps that the leg is in the air

TLDivFactor = 15; // number of steps that the leg is on the ground
StepsInGait = 20; // total steps
}
[/sourcecode]

For a complete Gait cycle (e.g. from step 1 to step 20), we will go
through loop() function 20 times, each time we call
GaitCalculate(), and then increment ‘GaitStep’ variable.

[sourcecode language=”cpp”]
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;

}
[/sourcecode]