Please use the reference section to see the actual link to the code and firmware 

// Dino Asmar
// EE400D
// MicroSpot Firmware
//************************************* library for servos
#include 
#include 
//******************************************************* setup servos
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOPOSMAX 2300
#define SERVOPOSMIN 500
//********************************************** connect to i2c DRIVER
int Servo_Knee[4] = {0, 1, 2, 3}; // -----> BL ,BR ,FL ,FR
int Servo_Thigh[4] = {4, 5, 6, 7}; // -----> BL, BR, FL, FR
int Servo_Left_Right[4] = {8, 9, 10, 11}; // -----> BL, BR, FL, FR
//*********************************** library for set commands
#include 
#include 

//************************************** setup library arxrobot for add comammds
ArxRobot ArxRobot;
const uint8_t CMD_LIST_SIZE = 1; // we are adding 1 commands coming soon we should added to 1
#define MOTION 0x40
uint8_t Action = 0; //This Global variable will hole the action that user currently selected.
#define FWD 1
#define BWD 2
#define LFT 3
#define RGT 4
#define STND 5
//***************************************************************** set commands
bool motionHandler(uint8_t cmd, uint8_t param[], uint8_t n)
{
PORTD |= 0b00100000; //Turning ON the LED
delay(1000);
PORTD &= 0b11011111; //Making sure that LED is OFF
delay(1000);
if (param[0] == 0x00) { //Here I am assigning the Action to become FWD since select option that moves forward has a 0x00 ID.
Action = FWD;
}
if (param[0] == 0x01) {
Action = BWD;
}
if (param[0] == 0x02) {
Action = LFT;
}
if (param[0] == 0x03) {
Action = RGT;
}
if (param[0] == 0x04) {
Action = STND;
}

return false;
}

ArxRobot::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{MOTION, motionHandler}};
//************************************* ultrasonic connect to 3dot

// Previous variable saves the previous angle and save it, and the new angles startes from the previous angle. For example;
// when micro servo number1 moves to the 40 degrees, next move of the micro servo number1 will be 40 degrees plus another 40 degrees. This method will
// help the micro servo to hold the old angle and apply the new angle to the move.
//int previous = 0;
void setup()
{
Serial.begin(9600);

//************************************************** setup for ArxRbot library and active it
ArxRobot.begin();
ArxRobot.setCurrentLimit(60);
ArxRobot.setOnCommand(onCommand , CMD_LIST_SIZE);
//**************************************************
DDRD |= 0b00100000; //Setting PDF5 to be an output
PORTD &= 0b11011111; //Making sure that LED is OFF
//********************************************* setup function about servos
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(60);

delay(10);

}

void loop()
{
ArxRobot.loop();

if (Action == FWD) {
forward();
}
else if (Action == BWD) {
backward();
}
else if (Action == LFT) {
left();
}
else if (Action == RGT) {
right();
}
else if (Action == STND) {
cte();
}

}
void cte()
{
//*************************************************************** Orientation of knees and thigh
Position(Servo_Knee[0], 0, 180 ); // First data represent the angle of the micro servo, and second data value represent which micro servos are going to receive the data.
Position(Servo_Knee[2], 0, 180 );

Position(Servo_Knee[1], 180, 0 );
Position(Servo_Knee[3], 180, 0 );

Position(Servo_Thigh[0], 0, 180 );
Position(Servo_Thigh[2], 0, 180 );

Position(Servo_Thigh[1], 180, 0);
Position(Servo_Thigh[3], 180, 0);

// Position(Servo_Left_Right[0, 1, 2, 3], 0, 90 );
}

void forward()
{
// Those comments out commands are mainly for the debugging porpuses and the way that I can debug the code is by using those commands. Leave it comment out
//previous = 40;
Position(Servo_Knee[0], 40, 60 );
//previous = 140;
Position(Servo_Knee[3], 140, 120 );

// Position(Servo_Knee[1,3],Position (120) );
// delay(500);

//previous = 140;
Position(Servo_Thigh[0], 140, 120 );
//previous = 40;
Position(Servo_Thigh[3], 40, 60 );

// Position(Servo_Thigh[1,3],Position (60) );
// delay(500);

//previous = 60;
Position(Servo_Knee[0], 60, 40 );
//previous = 120;
Position(Servo_Knee[3], 120, 140 );

// Position(Servo_Thigh[1,3],Position (140) );
// delay(500);

//previous = 140;
Position(Servo_Knee[1], 140, 120 );
//previous = 40;
Position(Servo_Knee[2], 40, 60);

// delay(500);

//previous = 120;
Position(Servo_Thigh[0], 120, 140 );
//previous = 60;
Position(Servo_Thigh[3], 60, 40 );

// Position(Servo_Knee[1,3],Position (40) );
// delay(500);

//previous = 140;
Position(Servo_Thigh[1], 140, 120 );
//previous = 40;
Position(Servo_Thigh[2], 40, 60 );

// delay(500);

//previous = 120;
Position(Servo_Knee[1], 120, 140 );
//previous = 60;
Position(Servo_Knee[2], 60, 40 );

// delay(500);

//previous = 40;
Position(Servo_Knee[0], 40, 60 );
//previous = 140;
Position(Servo_Knee[3], 140, 120 );

//previous = 120;
Position(Servo_Thigh[1], 120, 140 );
//previous = 60;
Position(Servo_Thigh[2], 60, 40 );

//previous = 60;
Position(Servo_Knee[0], 60, 40 );
//previous = 120;
Position(Servo_Knee[3], 120, 140);

// delay(500);
}

void backward()
{
//previous = 40;
Position(Servo_Knee[0], 40, 60);
//previous = 140;
Position(Servo_Knee[3], 140, 120 );

// Position(Servo_Knee[1,3],Position (120) );
// delay(500);

//previous = 140;
Position(Servo_Thigh[0], 140, 160 );
//previous = 40;
Position(Servo_Thigh[3], 40, 20 );

// Position(Servo_Thigh[1,3],Position (60) );
// delay(500);

//previous = 60;
Position(Servo_Knee[0], 60, 40 );
//previous = 120;
Position(Servo_Knee[3], 120, 140);

// Position(Servo_Thigh[1,3],Position (140) );
// delay(500);

//previous = 140;
Position(Servo_Knee[1], 140, 120 );
//previous = 40;
Position(Servo_Knee[2], 40, 60 );

// delay(500);

//previous = 160;
Position(Servo_Thigh[0], 160, 140);
//previous = 20;
Position(Servo_Thigh[3], 20, 40 );

// Position(Servo_Knee[1,3],Position (40) );
// delay(500);

//previous = 140;
Position(Servo_Thigh[1], 140, 160 );
//previous = 40;
Position(Servo_Thigh[2], 40, 20 );

// delay(500);

//previous = 120;
Position(Servo_Knee[1], 120, 140 );
//previous = 60;
Position(Servo_Knee[2], 60, 40 );

// delay(500);

//previous = 40;
Position(Servo_Knee[0], 40, 60);
//previous = 140;
Position(Servo_Knee[3], 140, 120 );

//previous = 160;
Position(Servo_Thigh[1], 160, 140 );
//previous = 20;
Position(Servo_Thigh[2], 20, 40);

//previous = 60;
Position(Servo_Knee[0], 60, 40 );
//previous = 120;
Position(Servo_Knee[3], 120, 140);
}

void left()
{
for (int y = 0; y <= 1; y++)
{
int Lefts = 2 * y + 1;
//previous = 40;
Position(Servo_Knee[Lefts], 40, 60 );
//previous = 90;
Position(Servo_Left_Right[Lefts], 90, 100 );

//previous = 60;
Position(Servo_Knee[Lefts], 60, 40 );
//previous = 100;
Position(Servo_Left_Right[Lefts], 100, 90);

int Rights = 2 * y;
//previous = 140;
Position(Servo_Knee[Rights], 140, 120);
//previous = 90;
Position(Servo_Left_Right[Rights], 90, 80);

//previous = 120;
Position(Servo_Knee[Rights], 120, 140);
//previous = 80;
Position(Servo_Left_Right[Rights], 80, 90 );
}
}
void right()
{
for (int y = 0; y <= 1; y++)
{
int Lefts = 2 * y + 1;
//previous = 40;
Position(Servo_Knee[Lefts], 40, 60 );
//previous = 90;
Position(Servo_Left_Right[Lefts], 90, 80 );

//previous = 60;
Position(Servo_Knee[Lefts], 60, 40);
//previous = 80;
Position(Servo_Left_Right[Lefts], 80, 90);

int Rights = 2 * y;
//previous = 140;
Position(Servo_Knee[Rights], 140, 120 );
//previous = 90;
Position(Servo_Left_Right[Rights], 90, 100 );

//previous = 120;
Position(Servo_Knee[Rights], 120, 140 );
//previous = 100;
Position(Servo_Left_Right[Rights], 100, 90 );
}
}

void Position( int leg, int previous , int ang)
{
int pulse = map(ang, 0, 180, SERVOPOSMIN, SERVOPOSMAX);
//Serial.print("Angle :");
//Serial.print(ang);
//Serial.print("pulse :");
//Serial.print(pulse);
//return pulse;

int Previous_F = map(previous, 0, 180, SERVOPOSMIN, SERVOPOSMAX);
//Serial.print("previous :");
//Serial.print(previous);
//Serial.print("Previous_F :");
//Serial.println(Previous_F);

//return Previous_F ; return pulse;

for (Previous_F; Previous_F < pulse; Previous_F++)
{
pwm.writeMicroseconds(leg, Previous_F);
// delay(3);
}

for (Previous_F; Previous_F > pulse; Previous_F--)
{
pwm.writeMicroseconds(leg, Previous_F);
// delay(1);
}
// return Previous_F && pulse;

}