//***********************************************
//
//      SPHERICAL QUADRUPED ARDUINO ROBOT
//
//***********************************************

//************************************ LIBRAIRIES
#include "global.h"
#include <Wire.h>
#include <Adafruit_NeoPixel.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Adafruit_NeoPixel strip(Led_Count, Led_Pin, NEO_GRB + NEO_KHZ800);

//***************************************** SETUP
void setup() {

  // US
  pinMode (Ultrasonic_Sensor_Trigger_Pin, OUTPUT);
  digitalWrite(Ultrasonic_Sensor_Trigger_Pin, LOW);
  pinMode (Ultrasonic_Sensor_Echo_Pin, INPUT);

  // LED
  strip.begin();
  strip.show();
  strip.setBrightness(50);

  // SERVO
  pwm.begin();
  pwm.setPWMFreq(60);
  Servo_Start_Position();
}

//****************************************** LOOP
void loop() {

  switch (Sequence) {
    case Stand_By:
      if (Ultrasonic_Sensor() < Distance_To_Activate_Switch) Sequence = Legs_Deployment;
      break;

    case Legs_Deployment:
      Door(Opening);
      Legs_Partially_Unfolded(); // DEPLOY THE LEGS UP TO TOUCH THE GROUND
      Legs_Unfolded(); // DEPLOY THE LEGS TO LIFT THE ROBOT

    case Moving_Forward:
      Led_Walking();
      while (Ultrasonic_Sensor() > Distance_To_Activate_Switch) { // CONTINUE TO WALK UNTIL SWITCH ACTIVATION
        if (Ultrasonic_Sensor() > Distance_Obstacle) walk();
        else if (Head_Rotation() == Left) Rotation(Left);
        else Rotation(Right);
      }

    case Folding_Legs:
      Legs_Partially_Folded(); // FOLD THE LEGS UP TO PUT DOWN THE ROBOT
      Legs_Folded(); // FOLD THE LEGS UP TO THE RETRACTED POSITION
      Door(Closing);

    case Restart:
      Sequence = Stand_By;
      strip.clear();
      strip.show();
  }
}

//******************************** START POSITION
void Servo_Start_Position() {

  Hip_Centered();
  pwm.setPWM(Head_Servo_Pin, 0, Head_Servo_Start_Position);
  Legs_Folded();
  pwm.setPWM(Door_Servo_Pin, 0, Door_Servo_Close);
  delay(1000);
}

//****************************************** HEAD
int Head_Rotation() {

  int Head_Angle = Head_Servo_Start_Position;
  long Distance_Left, Distance_Right;

  for (byte rotation_step = 0; rotation_step < 4; rotation_step++) {

    // MEASURMENT
    if (rotation_step == 1 ) {
      Distance_Left = Ultrasonic_Sensor();
      delay(1000);
    }
    if (rotation_step == 3) {
      Distance_Right = Ultrasonic_Sensor();
      delay(1000);
    }

    // LED ANIMATION
    if (rotation_step == 0 || rotation_step == 3 ) Led_Door_Head(Rotation_Left, Head_Led);
    else if (rotation_step == 1) Led_Door_Head(Rotation_Right, Head_Led);


    // HEAD ROTATION
    for (byte pitch = 0; pitch < 20; pitch++) {
      pwm.setPWM(Head_Servo_Pin, 0, Head_Angle);
      if (rotation_step == 0 || rotation_step == 3 ) Head_Angle += 2;
      else Head_Angle -= 2;
      delay(Head_Servo_Speed);
    }
  }

  // SELECT THE FREE SIDE
  if (Distance_Left > Distance_Right) return Left;
  else return Right;
}

//****************************************** KNEE
// FOLDED
void Legs_Folded() {

  Knee_Servo_Front_Right (Knee_Servo_Front_Right_Bent);
  Knee_Servo_Front_Left (Knee_Servo_Front_Left_Bent);
  Knee_Servo_Rear_Right (Knee_Servo_Rear_Right_Bent);
  Knee_Servo_Rear_Left (Knee_Servo_Rear_Left_Bent);
  delay(2000);
}

// PARTIALLY UNFOLDED
void Legs_Partially_Unfolded() {

  Led_Leg();

  Knee_Servo_Front_Right (Knee_Servo_Front_Right_Partially_Unfolded);
  Knee_Servo_Front_Left (Knee_Servo_Front_Left_Partially_Unfolded);
  Knee_Servo_Rear_Right (Knee_Servo_Rear_Right_Partially_Unfolded);
  Knee_Servo_Rear_Left (Knee_Servo_Rear_Left_Partially_Unfolded);
  delay(2000);
}

// KNEE UNFOLDED LOW SPEED
void Legs_Unfolded() {

  for (byte a = 0; a < 20; a++) { // LOOP TO REDUCE THE SERVOMOTOR SPEED
    Knee_Servo_Front_Right(map(a, 0, 20, Knee_Servo_Front_Right_Partially_Unfolded, Knee_Servo_Front_Right_Unfolded));
    Knee_Servo_Front_Left(map(a, 0, 20, Knee_Servo_Front_Left_Partially_Unfolded, Knee_Servo_Front_Left_Unfolded));
    Knee_Servo_Rear_Right(map(a, 0, 20, Knee_Servo_Rear_Right_Partially_Unfolded, Knee_Servo_Rear_Right_Unfolded));
    Knee_Servo_Rear_Left(map(a, 0, 20, Knee_Servo_Rear_Left_Partially_Unfolded, Knee_Servo_Rear_Left_Unfolded));
    delay(Knee_Servo_Speed);
  }
  delay(2000);
}

// KNEE BENT LOW SPEED
void Legs_Partially_Folded() {

  Led_Leg();

  for (byte a = 0; a < 20; a++) { // LOOP TO REDUCE THE SERVOMOTOR SPEED
    Knee_Servo_Front_Right(map(a, 0, 20, Knee_Servo_Front_Right_Unfolded, Knee_Servo_Front_Right_Partially_Unfolded));
    Knee_Servo_Front_Left(map(a, 0, 20, Knee_Servo_Front_Left_Unfolded, Knee_Servo_Front_Left_Partially_Unfolded));
    Knee_Servo_Rear_Right(map(a, 0, 20, Knee_Servo_Rear_Right_Unfolded, Knee_Servo_Rear_Right_Partially_Unfolded));
    Knee_Servo_Rear_Left(map(a, 0, 20, Knee_Servo_Rear_Left_Unfolded, Knee_Servo_Rear_Left_Partially_Unfolded));
    delay(Knee_Servo_Speed);
  }
  delay(2000);
}

// COMMAND
void Knee_Servo_Front_Right (int pulse) {

  pwm.setPWM(Knee_Servo_Front_Right_Pin, 0, pulse);
}

void Knee_Servo_Front_Left (int pulse) {

  pwm.setPWM(Knee_Servo_Front_Left_Pin, 0, pulse);
}

void Knee_Servo_Rear_Right (int pulse) {

  pwm.setPWM(Knee_Servo_Rear_Right_Pin, 0, pulse);
}

void Knee_Servo_Rear_Left (int pulse) {

  pwm.setPWM(Knee_Servo_Rear_Left_Pin, 0, pulse);
}

//******************************************* HIP
// CENTERED
void Hip_Centered() {

  Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered);
  Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered);
  Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered);
  Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered);
}

// COMMAND
void Hip_Servo_Front_Right (int pulse) {

  pwm.setPWM(Hip_Servo_Front_Right_Pin, 0, pulse);
}

void Hip_Servo_Front_Left (int pulse) {

  pwm.setPWM(Hip_Servo_Front_Left_Pin, 0, pulse);
}

void Hip_Servo_Rear_Right (int pulse) {

  pwm.setPWM(Hip_Servo_Rear_Right_Pin, 0, pulse);
}

void Hip_Servo_Rear_Left (int pulse) {

  pwm.setPWM(Hip_Servo_Rear_Left_Pin, 0, pulse);
}

//****************************************** WALK
void walk() {

  // SEE INSTRUCTABLES DETAIL FOR THE WALKING STRATEGY

  Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded - Knee_Vertical_Amplitude);
  Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded - Knee_Vertical_Amplitude);
  delay(delay_knee);

  Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered - Hip_Servo_Front_Right_Move);
  Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered + Hip_Servo_Rear_Left_Move);
  delay(delay_hip);

  Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded + Knee_Vertical_Amplitude);
  Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded + Knee_Vertical_Amplitude);
  delay(delay_knee);

  Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered);
  Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered);
  delay(delay_hip);

  Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded);
  Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded);
  delay(delay_knee);

  Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded - Knee_Vertical_Amplitude);
  Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded - Knee_Vertical_Amplitude);
  delay(delay_knee);

  Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered + Hip_Servo_Front_Left_Move);
  Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered - Hip_Servo_Rear_Right_Move);
  delay(delay_hip);

  Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded + Knee_Vertical_Amplitude);
  Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded + Knee_Vertical_Amplitude);
  delay(delay_knee);

  Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered);
  Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered);
  delay(delay_hip);

  Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded);
  Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded);
  delay(delay_knee);
}

//************************************** ROTATION
void Rotation (byte Direction) {

  // SEE INSTRUCTABLES DETAIL FOR THE ROTATION STRATEGY

  for (byte a = 0; a < 4; a++) {
    Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded - Knee_Vertical_Amplitude);
    Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded - Knee_Vertical_Amplitude);
    delay(delay_knee);

    if (Direction == Right) {
      Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered - Hip_Servo_Front_Right_Move);
      Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered - Hip_Servo_Rear_Left_Move);
    }
    else {
      Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered + Hip_Servo_Front_Right_Move);
      Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered + Hip_Servo_Rear_Left_Move);
    }
    delay(delay_hip);

    Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded + Knee_Vertical_Amplitude);
    Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded + Knee_Vertical_Amplitude);
    delay(delay_knee);

    Hip_Servo_Front_Right (Hip_Servo_Front_Right_Centered);
    Hip_Servo_Rear_Left (Hip_Servo_Rear_Left_Centered);
    delay(delay_hip);

    Knee_Servo_Front_Right(Knee_Servo_Front_Right_Unfolded);
    Knee_Servo_Rear_Left(Knee_Servo_Rear_Left_Unfolded);
    delay(delay_knee);

    Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded - Knee_Vertical_Amplitude);
    Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded - Knee_Vertical_Amplitude);
    delay(delay_knee);

    if (Direction == Right) {
      Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered - Hip_Servo_Front_Left_Move);
      Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered - Hip_Servo_Rear_Right_Move);
    }
    else {
      Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered + Hip_Servo_Front_Left_Move);
      Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered + Hip_Servo_Rear_Right_Move);
    }
    delay(delay_hip);

    Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded + Knee_Vertical_Amplitude);
    Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded + Knee_Vertical_Amplitude);
    delay(delay_knee);

    Hip_Servo_Front_Left (Hip_Servo_Front_Left_Centered);
    Hip_Servo_Rear_Right (Hip_Servo_Rear_Right_Centered);
    delay(delay_hip);

    Knee_Servo_Front_Left(Knee_Servo_Front_Left_Unfolded);
    Knee_Servo_Rear_Right(Knee_Servo_Rear_Right_Unfolded);
    delay(delay_knee);
  }
}

//***************************** ULTRASONIC SENSOR
long Ultrasonic_Sensor() {

  digitalWrite(Ultrasonic_Sensor_Trigger_Pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Ultrasonic_Sensor_Trigger_Pin, LOW);
  Measure = pulseIn(Ultrasonic_Sensor_Echo_Pin, HIGH);
  Distance_mm = Measure / 58;
  delay(100);
  return Distance_mm;
}

//****************************************** DOOR
void Door (int State) {

  if (State == Closing) {
    Led_Door_Head(Rotation_Left, Door_Led);

    for (byte a = 0; a < 20; a++) { // LOOP TO REDUCE THE SERVOMOTOR SPEED
      pwm.setPWM(Door_Servo_Pin, 0, map(a, 0, 20, Door_Servo_Open, Door_Servo_Close));
      delay(Door_Servo_Speed);
    }
  }
  else {
    Led_Door_Head(Rotation_Right, Door_Led);

    for (byte a = 0; a < 20; a++) { // LOOP TO REDUCE THE SERVOMOTOR SPEED
      pwm.setPWM(Door_Servo_Pin, 0, map(a, 0, 20, Door_Servo_Close, Door_Servo_Open));
      delay(Door_Servo_Speed);
    }
  }
  delay(1000);
}

//******************************************* LED
// DOOR & HEAD
void Led_Door_Head(int Direction, int Item) {

  // CREATE A LEDS ANIMATION WHO TURN AROUND THE ROBOT ON THE SAME DIRECTION THAT THE MECHANISM

  if (Direction == Rotation_Left) {
    for (byte a = 0; a < 3; a++) {
      for (byte b = 0; b <= 3; b++) {
        strip.clear();
        if (Item == Head_Led) strip.setPixelColor(b, 0, 0, 250);
        else strip.setPixelColor(b, 250, 0, 0);
        strip.show();
        delay(100);
      }
    }
  }
  else { // ROTATION RIGHT
    for (byte a = 0; a < 3; a++) {
      for (byte b = 3; b >= 0; b--) {
        strip.clear();
        if (Item == Head_Led) strip.setPixelColor(b, 0, 0, 250);
        else strip.setPixelColor(b, 250, 0, 0);
        strip.show();
        delay(100);
      }
    }
  }
  strip.clear();
  strip.show();
}

// LEG
void Led_Leg() {
  strip.clear();
  strip.setPixelColor(0, 250, 0, 0);
  strip.setPixelColor(1, 250, 0, 0);
  strip.setPixelColor(2, 250, 0, 0);
  strip.setPixelColor(3, 250, 0, 0);
  strip.show();
}

// WALK
void Led_Walking() {
  strip.clear();
  strip.setPixelColor(0, 0, 250, 0);
  strip.setPixelColor(1, 0, 250, 0);
  strip.setPixelColor(2, 0, 250, 0);
  strip.setPixelColor(3, 0, 250, 0);
  strip.show();
  delay(2000);
}

#include "Arduino.h"

// ROBOT
enum {Stand_By, Legs_Deployment, Moving_Forward, Folding_Legs, Restart};
byte Sequence = Stand_By;
enum {Rotation_Left, Rotation_Right, Head_Led, Door_Led, Opening, Closing, Right, Left};
const byte Distance_To_Activate_Switch (4);
const byte Distance_Obstacle (20);

// LED
const byte Led_Pin (12);
const byte Led_Count (4);

// ULTRASONIC SENSOR
const byte Ultrasonic_Sensor_Trigger_Pin (6);
const byte Ultrasonic_Sensor_Echo_Pin (8);
long Measure = 0;
long Distance_mm = 0;

// DOOR
const byte Door_Servo_Pin (1);
const int Door_Servo_Open (330);
const int Door_Servo_Close (205);
const int Door_Servo_Speed (150);

// HIP
const byte Hip_Servo_Front_Right_Pin (14);
const int Hip_Servo_Front_Right_Centered (283);
const int Hip_Servo_Front_Right_Move (70);

const byte Hip_Servo_Front_Left_Pin (12);
const int Hip_Servo_Front_Left_Centered (321);
const int Hip_Servo_Front_Left_Move (66);

const byte Hip_Servo_Rear_Right_Pin (3);
const int Hip_Servo_Rear_Right_Centered (347);
const int Hip_Servo_Rear_Right_Move (62);

const byte Hip_Servo_Rear_Left_Pin (11);
const int Hip_Servo_Rear_Left_Centered (268);
const int Hip_Servo_Rear_Left_Move (64);

const int delay_hip (400);

// KNEE
const byte Knee_Servo_Front_Right_Pin (2);
const int Knee_Servo_Front_Right_Bent (220);
const int Knee_Servo_Front_Right_Partially_Unfolded (343);
const int Knee_Servo_Front_Right_Unfolded (429);

const byte Knee_Servo_Front_Left_Pin (13);
const int Knee_Servo_Front_Left_Bent (216);
const int Knee_Servo_Front_Left_Partially_Unfolded (335);
const int Knee_Servo_Front_Left_Unfolded (423);

const byte Knee_Servo_Rear_Right_Pin (15);
const int Knee_Servo_Rear_Right_Bent (211);
const int Knee_Servo_Rear_Right_Partially_Unfolded (340);
const int Knee_Servo_Rear_Right_Unfolded (408);

const byte Knee_Servo_Rear_Left_Pin (4);
const int Knee_Servo_Rear_Left_Bent (200);
const int Knee_Servo_Rear_Left_Partially_Unfolded (349);
const int Knee_Servo_Rear_Left_Unfolded (436);

const byte Knee_Servo_Speed (130);
const byte delay_knee (150);
const byte Knee_Vertical_Amplitude (8);

// HEAD
const byte Head_Servo_Pin (0);
const int Head_Servo_Start_Position (339);
const int Head_Servo_Speed (100);