// Emily_Robot_II program, quad walking dog
// Last worked on January 2023
// Uses same PC board as Emily

#define FRONT_HIP_L   A9    // These are the pin numbers on the Teensy 4.0
#define FRONT_FEMUR_L 3
#define FRONT_TIBIA_L A8

#define FRONT_HIP_R   4
#define FRONT_FEMUR_R A7
#define FRONT_TIBIA_R 5

#define BACK_HIP_L   A6
#define BACK_FEMUR_L 6
#define BACK_TIBIA_L A3

#define BACK_HIP_R   9
#define BACK_FEMUR_R A2
#define BACK_TIBIA_R A1

#define LED     13
#define RED_LED A0

#define STRAIGHT 0
#define RIGHT    1
#define LEFT     2

// Generic
//

float femur_BL[17] = { 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955 };
float tibia_BL[17] = { 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265 };

float femur_FL[17] = { 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485 };
float tibia_FL[17] = { 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505 };

float femur_BR[17] = { 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875 };
float tibia_BR[17] = { 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595 };

float femur_FR[17] = { 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325 };
float tibia_FR[17] = { 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425 };

// Walk
//                                                                                                                        ----------------------  
float e_femur_BL[17] = { 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955 };
float e_tibia_BL[17] = { 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265 };
//                        ----------------------
float e_femur_FL[17] = { 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485 };
float e_tibia_FL[17] = { 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505 };
//                                                        ----------------------
float e_femur_BR[17] = { 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325, 56.785, 52.655, 49.005, 45.875 };
float e_tibia_BR[17] = { 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425, 51.505, 56.085, 61.125, 66.595 };
//                                                                                        ----------------------
float e_femur_FR[17] = { 61.325, 56.785, 52.655, 49.005, 45.875, 43.335, 41.465, 40.395, 13.485, 39.755, 79.475, 89.275, 82.955, 77.045, 71.475, 66.225, 61.325 };
float e_tibia_FR[17] = { 47.425, 51.505, 56.085, 61.125, 66.595, 72.465, 78.755, 85.545, 71.505, 29.115, 5.515, 36.655, 37.265, 38.745, 40.985, 43.895, 47.425 };

// Turn right
// Right gets shorter
float e_femur_BL_right[17] = { 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505 };
float e_tibia_BL_right[17] = { 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225 };

float e_femur_FL_right[17] = { 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165 };
float e_tibia_FL_right[17] = { 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625 };

float e_femur_BR_right[17] = { 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735 };
float e_tibia_BR_right[17] = { 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635 };

float e_femur_FR_right[17] = { 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825 };
float e_tibia_FR_right[17] = { 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855 };

// Turn left
// Left gets shorter
float e_femur_BL_left[17] = { 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505 };
float e_tibia_BL_left[17] = { 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225 };

float e_femur_FL_left[17] = { 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165 };
float e_tibia_FL_left[17] = { 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625 };

float e_femur_BR_left[17] = { 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825, 32.875, 29.195, 25.805, 22.735 };
float e_tibia_BR_left[17] = { 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855, 45.585, 49.485, 53.505, 57.635 };

float e_femur_FR_left[17] = { 36.825, 32.875, 29.195, 25.805, 22.735, 20.005, 17.635, 15.645, 4.165, 24.215, 54.565, 59.245, 54.505, 49.865, 45.345, 40.985, 36.825 };
float e_tibia_FR_left[17] = { 41.855, 45.585, 49.485, 53.505, 57.635, 61.835, 66.095, 70.375, 68.625, 40.535, 18.225, 26.775, 29.225, 31.975, 35.015, 38.325, 41.855 };


//float e_hip_BL[17] = { 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0 };
//float e_hip_FL[17] = { 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0 };
//float e_hip_BR[17] = { 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0 };
//float e_hip_FR[17] = { 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0 };

//float e_hip_BL[17] = { -4.0, -4.0, -4.0, 0.0, 4.0, 4.0, 4.0, 0.0, 4.0, 4.0, 4.0, 0.0, -4.0, -4.0, -4.0, 0.0, -4.0 };
//float e_hip_FL[17] = { -4.0, -4.0, -4.0, 0.0, 4.0, 4.0, 4.0, 0.0, 4.0, 4.0, 4.0, 0.0, -4.0, -4.0, -4.0, 0.0, -4.0 };
//float e_hip_BR[17] = { -4.0, -4.0, -4.0, 0.0, 4.0, 4.0, 4.0, 0.0, 4.0, 4.0, 4.0, 0.0, -4.0, -4.0, -4.0, 0.0, -4.0 };
//float e_hip_FR[17] = { -4.0, -4.0, -4.0, 0.0, 4.0, 4.0, 4.0, 0.0, 4.0, 4.0, 4.0, 0.0, -4.0, -4.0, -4.0, 0.0, -4.0 };

float e_hip_BL[17] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
float e_hip_FL[17] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
float e_hip_BR[17] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
float e_hip_FR[17] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

// Offsets updated March 8, 2023, accurace probably not better than 1 degree
float BACK_femur_L_Offset = 1.5, BACK_femur_R_Offset = -2.0;
float BACK_tibia_L_Offset = -3.0, BACK_tibia_R_Offset = -2.5;
float BACK_hip_L_Offset = 4.0, BACK_hip_R_Offset = 0.0;
float FRONT_femur_L_Offset = -1.75, FRONT_femur_R_Offset = 1.0;
float FRONT_tibia_L_Offset = 5.75, FRONT_tibia_R_Offset = 0.0;
float FRONT_hip_L_Offset = 0.0, FRONT_hip_R_Offset = 0.0;

float uSec_factor = 3.11526, total;
bool stringComplete = false;
String inputString = "";
bool int_On = false;   // for no interrupts
bool all_stop = false; // for emergency
bool debug = true;
byte b;

// These added for 3 dimensions
//float legLength, Zlength, thetaA, J, K, theta1, theta2, theta3;
//float femurLength = 187.5;
//float tibiaLength = 150.0;
float coxaLength = 27.54;
float xx[16] = {-83.32, -66.66, -49.99, -33.32, -16.66, 0.0, 16.67, 33.33, 50.0, 66.67, 83.33, 100.0, 100.0, 0.0, -100.0, -100.0};
float yy[16] = {220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, 198.0, 198.0, 198.0, 220.0};

// The timers used to drive the Servo Gearboxes
IntervalTimer timerLFront, timerLBack, timerRFront, timerRBack;

// For each Servo Gearbox a to and from angle in usec, and temporary value
volatile int usec_from_FFL, usec_to_FFL, value_FFL;
// The count of 20 millisecond periods that determines the time of travel
volatile int temp_count_FFL, count_FFL;

volatile int usec_from_FTL, usec_to_FTL, value_FTL;
volatile int temp_count_FTL, count_FTL;
volatile int usec_from_FFR, usec_to_FFR, value_FFR;
volatile int temp_count_FFR, count_FFR;
volatile int usec_from_FTR, usec_to_FTR, value_FTR;
volatile int temp_count_FTR, count_FTR;
volatile int usec_from_FHL, usec_to_FHL, value_FHL;
volatile int temp_count_FHL, count_FHL;
volatile int usec_from_FHR, usec_to_FHR, value_FHR;
volatile int temp_count_FHR, count_FHR;

volatile int usec_from_BFL, usec_to_BFL, value_BFL;
volatile int temp_count_BFL, count_BFL;
volatile int usec_from_BTL, usec_to_BTL, value_BTL;
volatile int temp_count_BTL, count_BTL;
volatile int usec_from_BFR, usec_to_BFR, value_BFR;
volatile int temp_count_BFR, count_BFR;
volatile int usec_from_BTR, usec_to_BTR, value_BTR;
volatile int temp_count_BTR, count_BTR;
volatile int usec_from_BHL, usec_to_BHL, value_BHL;
volatile int temp_count_BHL, count_BHL;
volatile int usec_from_BHR, usec_to_BHR, value_BHR;
volatile int temp_count_BHR, count_BHR;

// usec values to drive Servo Gearboxes at full speed, or flag using speed
volatile int usec_FFe_L, usec_FTb_L, usec_FFe_R, usec_FTb_R, usec_FHp_L, usec_FHp_R;
volatile int usec_BFe_L, usec_BTb_L, usec_BFe_R, usec_BTb_R, usec_BHp_L, usec_BHp_R;
int i, the_count, j = 0;

// The 3 main functions
// int BACK_femur_L_uSec(float angle)                     Given an angle, compute the uSec value for a servo motor
// void moveToAngle(float angle, float fangle, int leg)   Given an angle, move leg to that angle. Tibia needs the femur angle as well
// void front_Servos_L()                                  A Timer function, which is called every 20 msec.

void setup() 
{
    Serial.begin(115200);
    pinMode(LED, OUTPUT);     // This is pin 13
    pinMode(RED_LED, OUTPUT);
    digitalWrite(LED, HIGH);
    delay(4000);
    pinMode(FRONT_FEMUR_L, OUTPUT);
    pinMode(FRONT_TIBIA_L, OUTPUT);
    pinMode(FRONT_FEMUR_R, OUTPUT);
    pinMode(FRONT_TIBIA_R, OUTPUT);
    pinMode(FRONT_HIP_L, OUTPUT);
    pinMode(FRONT_HIP_R, OUTPUT);
    pinMode(BACK_FEMUR_L, OUTPUT);
    pinMode(BACK_TIBIA_L, OUTPUT);
    pinMode(BACK_FEMUR_R, OUTPUT);
    pinMode(BACK_TIBIA_R, OUTPUT);
    pinMode(BACK_HIP_L, OUTPUT);
    pinMode(BACK_HIP_R, OUTPUT);
   

    // ***************************************************************************
    // Turn on the 4 timers
    // Each timer operates the femur, tibia and hip of a single leg
    timerLFront.begin(front_Servos_L, 20000);
    timerLBack.begin(back_Servos_L, 20000);
    timerRFront.begin(front_Servos_R, 20000);
    timerRBack.begin(back_Servos_R, 20000);

    // Calculate our leg angles
    // ***************************************************************************
    inv_Kinematics(187.5, 165.0, 100.0, 2.2, 0.9);
    
    digitalWrite(RED_LED, HIGH);
    delay(1000);
    digitalWrite(RED_LED, LOW);

    // Testing
    //
    Z_inv_Kinematics(187.5, 165, 40.0, 4.5, 0.9);
    
//    while (true) {};
    
//    while (true)
//    {
//        moveToAngle(0.0,  FRONT_HIP_L);
//        moveToAngle(0.0,  FRONT_HIP_R);
//        moveToAngle(0.0,  BACK_HIP_L);
//        moveToAngle(0.0,  BACK_HIP_R);
//        do{
//            moveToAngle(0.0, FRONT_FEMUR_R);
//            moveToAngle(50.0, FRONT_TIBIA_R);
//            delay(4000);
//            moveToAngle(0.0, FRONT_FEMUR_R);
//            moveToAngle(60.0, FRONT_TIBIA_R);
//            delay(8000);
//            moveToAngle(0.0, FRONT_FEMUR_R);
//            moveToAngle(70.0, FRONT_TIBIA_R);
//            delay(4000);
//            moveToAngle(0.0, FRONT_FEMUR_R);
//            moveToAngle(60.0, FRONT_TIBIA_R);
//        
//            while (Serial.available() == 0) {};
//            b = Serial.read();
//          } while ( b != '\n');
//    }

    // Sitting position, 45 deg. femur, 45 deg tibia
    // ***************************************************************************
    
    moveToAngle(45.0, FRONT_FEMUR_L);  //45
    moveToAngle(45.0, FRONT_TIBIA_L);
    moveToAngle(45.0, FRONT_FEMUR_R);
    moveToAngle(45.0, FRONT_TIBIA_R);
    moveToAngle(0.0, FRONT_HIP_L);
    moveToAngle(0.0, FRONT_HIP_R);
    
    moveToAngle(45.0, BACK_FEMUR_L);  
    moveToAngle(45.0, BACK_TIBIA_L);
    moveToAngle(45.0, BACK_FEMUR_R);
    moveToAngle(45.0, BACK_TIBIA_R);
    moveToAngle(0.0, BACK_HIP_L);
    moveToAngle(0.0, BACK_HIP_R);
    delay(4000);
 //   while(true) {};

    // Stand Up
    // ***************************************************************************
    
//    moveToAngle(45.0, FRONT_TIBIA_L);
//    moveToAngle(45.0, FRONT_TIBIA_R);
//    moveToAngle(45.0, BACK_TIBIA_L);
//    moveToAngle(45.0, BACK_TIBIA_R);
//    delay(4000);
//    
    // while(true) {};
    // Move to first position
    // ***************************************************************************
    
    moveToAngle(e_femur_BL[0], BACK_FEMUR_L);
    moveToAngle(e_tibia_BL[0], BACK_TIBIA_L);
    moveToAngle(e_femur_FL[0], FRONT_FEMUR_L);
    moveToAngle(e_tibia_FL[0], FRONT_TIBIA_L);
    moveToAngle(e_femur_BR[0], BACK_FEMUR_R);
    moveToAngle(e_tibia_BR[0], BACK_TIBIA_R);
    moveToAngle(e_femur_FR[0], FRONT_FEMUR_R);
    moveToAngle(e_tibia_FR[0], FRONT_TIBIA_R);

    delay(2000);
  //  while(true) {};
}

void loop() 
{
    i = 0;
    the_count = 6;    // the_count * 20 msec. for each time slice
    while(i < 16)      // 16 time slices, 12 * 20 * 16 = 3840 msec.
    {
        Serial.println(i);
        usec_from_FFL = FRONT_femur_L_uSec(e_femur_FL[i]);   // Get microseconds needed for
        usec_to_FFL = FRONT_femur_L_uSec(e_femur_FL[i+1]);   // from and to angles
        count_FFL = the_count;                               // Set count_FL, number of 20 millisecond periods for speed
        temp_count_FFL = 0;                                  // Zero the temporary counter
        usec_FFe_L = 0;                                      // Set usec_Fe_L = 0, so speed will be used
  
        usec_from_FTL = FRONT_tibia_L_uSec(e_tibia_FL[i]);
        usec_to_FTL = FRONT_tibia_L_uSec(e_tibia_FL[i+1]);
        count_FTL = the_count;
        temp_count_FTL = 0;
        usec_FTb_L = 0;
  
        usec_from_FFR = FRONT_femur_R_uSec(e_femur_FR[i]);
        usec_to_FFR = FRONT_femur_R_uSec(e_femur_FR[i+1]);
        count_FFR = the_count;
        temp_count_FFR = 0;
        usec_FFe_R = 0;
  
        usec_from_FTR = FRONT_tibia_R_uSec(e_tibia_FR[i]);
        usec_to_FTR = FRONT_tibia_R_uSec(e_tibia_FR[i+1]);
        count_FTR = the_count;
        temp_count_FTR = 0;
        usec_FTb_R = 0;
  
        usec_from_FHL = FRONT_hip_L_uSec(e_hip_FL[i]);
        usec_to_FHL = FRONT_hip_L_uSec(e_hip_FL[i+1]);
        count_FHL = the_count;
        temp_count_FHL = 0;
        usec_FHp_L = 0;
  
        usec_from_FHR = FRONT_hip_R_uSec(e_hip_FR[i]);
        usec_to_FHR = FRONT_hip_R_uSec(e_hip_FR[i+1]);
        count_FHR = the_count;
        temp_count_FHR = 0;
        usec_FHp_R = 0;

        // ***********************************************************************************
        
        usec_from_BFL = BACK_femur_L_uSec(e_femur_BL[i]);   // Get microseconds needed for
        usec_to_BFL = BACK_femur_L_uSec(e_femur_BL[i+1]);   // from and to angles
        count_BFL = the_count;                                 // Set count_FL, number of 20 millisecond periods for speed
        temp_count_BFL = 0;                                    // Zero the temporary counter
        usec_BFe_L = 0;                                        // Set usec_Fe_L = 0, so speed will be used
  
        usec_from_BTL = BACK_tibia_L_uSec(e_tibia_BL[i]);
        usec_to_BTL = BACK_tibia_L_uSec(e_tibia_BL[i+1]);
        count_BTL = the_count;
        temp_count_BTL = 0;
        usec_BTb_L = 0;
  
        usec_from_BFR = BACK_femur_R_uSec(e_femur_BR[i]);
        usec_to_BFR = BACK_femur_R_uSec(e_femur_BR[i+1]);
        count_BFR = the_count;
        temp_count_BFR = 0;
        usec_BFe_R = 0;
  
        usec_from_BTR = BACK_tibia_R_uSec(e_tibia_BR[i]);
        usec_to_BTR = BACK_tibia_R_uSec(e_tibia_BR[i+1]);
        count_BTR = the_count;
        temp_count_BTR = 0;
        usec_BTb_R = 0;
  
        usec_from_BHL = BACK_hip_L_uSec(e_hip_BL[i]);
        usec_to_BHL = BACK_hip_L_uSec(e_hip_BL[i+1]);
        count_BHL = the_count;
        temp_count_BHL = 0;
        usec_BHp_L = 0;
  
        usec_from_BHR = BACK_hip_R_uSec(e_hip_BR[i]);
        usec_to_BHR = BACK_hip_R_uSec(e_hip_BR[i+1]);
        count_BHR = the_count;
        temp_count_BHR = 0;
        usec_BHp_R = 0;
  
        //while (temp_count_BFL != count_BFL)
        while ((temp_count_BTL != count_BTL) && (temp_count_BFL != count_BFL) &&      // this doesn't change checking all 8 motors
               (temp_count_BTR != count_BTR) && (temp_count_BFR != count_BFR) &&
               
               (temp_count_FTL != count_FTL) && (temp_count_FFL != count_FFL) &&
               (temp_count_FTR != count_FTR) && (temp_count_FFR != count_FFR))
        {
          //  Serial.print(micros()); Serial.print("  ");
          //  accel.getEvent(&event);
          //  pitchSum = smoothpitchSensorReadings(event.acceleration.x);
          //  rollSum = smoothrollSensorReadings(event.acceleration.y);
          //  Serial.println(micros());
          //  Serial.print("X: "); Serial.print(pitchSum); Serial.print("  ");
          //  Serial.print("Y: "); Serial.println(rollSum);
        };
        mySerial_Event();
        if (stringComplete) get_Master_command();
//        do{
//        Serial.println(i);
//        while (Serial.available() == 0) {};
//        b = Serial.read();
//        } while ( b != '\n');
        i++;
    }
    j++;
    Serial.print(j);
    Serial.print("  ");
    Serial.println(millis());
}


// ******************************************************************************
// Compute uSec pulse width for given angle
// Uses 3.11526 factor for each 1 degree off 1500 uSec center positiion
// 

int BACK_femur_L_uSec(float angle)
{
  int uSec = int(1500.0 - ((angle + BACK_femur_L_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int BACK_femur_R_uSec(float angle)
{
  int uSec = int(1500.0 + ((angle + BACK_femur_R_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int BACK_tibia_L_uSec(float angle)
{
  float comp = 90.0 - angle;
  int uSec = int(1500.0 - ((comp + BACK_tibia_L_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int BACK_tibia_R_uSec(float angle)
{
  float comp = 90.0 - angle;
  int uSec = int(1500.0 + ((comp + BACK_tibia_R_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int BACK_hip_L_uSec(float angle)
{
  int uSec = int(1500.0 + ((angle + BACK_hip_L_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int BACK_hip_R_uSec(float angle)
{
  int uSec = int(1500.0 + ((angle + BACK_hip_R_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

// ****************************************************************************************

int FRONT_femur_L_uSec(float angle)
{
  int uSec = int(1500.0 - ((angle + FRONT_femur_L_Offset) * uSec_factor) + 0.5);
  return uSec;
}

int FRONT_femur_R_uSec(float angle)
{
  int uSec = int(1500.0 + ((angle + FRONT_femur_R_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int FRONT_tibia_L_uSec(float angle)
{
  float comp = 90.0 - angle;
  int uSec = int(1500.0 - ((comp + FRONT_tibia_L_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int FRONT_tibia_R_uSec(float angle)
{
  float comp = 90.0 - angle;
  int uSec = int(1500.0 + ((comp + FRONT_tibia_R_Offset) * uSec_factor) + 0.5); 
  return uSec;
}

int FRONT_hip_L_uSec(float angle)
{ 
  int uSec = int(1500.0 + ((angle + FRONT_hip_L_Offset) * uSec_factor) + 0.5);   // Working 2/6/2023
  return uSec;
}

int FRONT_hip_R_uSec(float angle)
{
  int uSec = int(1500.0 + ((angle + FRONT_hip_R_Offset) * uSec_factor) + 0.5);   // Working 2/6/2023
  return uSec;
}

// ******************************************************************************
// Moves legs to given angles at full speed
void moveToAngle(float angle, int leg)
{
  switch (leg)
  {
    case FRONT_FEMUR_L:usec_FFe_L = FRONT_femur_L_uSec(angle);
                       break;
    case FRONT_TIBIA_L:usec_FTb_L = FRONT_tibia_L_uSec(angle);
                       break;
    case FRONT_FEMUR_R:usec_FFe_R = FRONT_femur_R_uSec(angle);
                       break;
    case FRONT_TIBIA_R:usec_FTb_R = FRONT_tibia_R_uSec(angle);
                       break;
    case FRONT_HIP_L:  usec_FHp_L = FRONT_hip_L_uSec(angle);
                       break;
    case FRONT_HIP_R:  usec_FHp_R = FRONT_hip_R_uSec(angle);
                       break;
                 
    case BACK_FEMUR_L:usec_BFe_L = BACK_femur_L_uSec(angle);
                      break;
    case BACK_TIBIA_L:usec_BTb_L = BACK_tibia_L_uSec(angle);
                      break;
    case BACK_FEMUR_R:usec_BFe_R = BACK_femur_R_uSec(angle);
                      break;
    case BACK_TIBIA_R:usec_BTb_R = BACK_tibia_R_uSec(angle);
                      break;
    case BACK_HIP_L:  usec_BHp_L = BACK_hip_L_uSec(angle);
                      break;
    case BACK_HIP_R:  usec_BHp_R = BACK_hip_R_uSec(angle);
                      break;
  }
}

// ******************************************************************************
// Functions the Timers call
void front_Servos_L()
{
    if (int_On) noInterrupts();
    // Femur ********************************************************************
    if (usec_FFe_L == 0)                   // If this is true then Servo Gearbox movement will take count_FL * 20 milliseconds
    {                                      // going to the angle defined by usec_to_FL from usec_from_FL
        if (temp_count_FFL != count_FFL)   // Note that it is possible to define a speed that exceeds the full speed value
        {                                  // of the Servo Gearbox
            temp_count_FFL += 1.0;
            value_FFL = usec_from_FFL + (int)(((float)temp_count_FFL/(float)count_FFL) * ((float)usec_to_FFL - (float)usec_from_FFL) + 0.5);
        }
        digitalWrite(FRONT_FEMUR_L, HIGH);
        delayMicroseconds(value_FFL);
        digitalWrite(FRONT_FEMUR_L, LOW);
    }
    else                                   // Otherwise just move to the desired angle, using usec value, at full speed 
    {                                      // The SG12-50 travels 48 deg/sec at 4.8 volts or 75 deg/sec at 7.4 volts
        digitalWrite(FRONT_FEMUR_L, HIGH); // The SG12-70 travels 34.29 deg/sec at 4.8 volts or 53.57 deg/sec at 7.4 volts
        delayMicroseconds(usec_FFe_L);
        digitalWrite(FRONT_FEMUR_L, LOW);
    }

    // Tibia ********************************************************************
    if (usec_FTb_L == 0)                   
    {                                     
        if (temp_count_FTL != count_FTL)   
        {                                  
            temp_count_FTL += 1.0;
            value_FTL = usec_from_FTL + (int)(((float)temp_count_FTL/(float)count_FTL) * ((float)usec_to_FTL - (float)usec_from_FTL) + 0.5);
        }
        digitalWrite(FRONT_TIBIA_L, HIGH);
        delayMicroseconds(value_FTL);
        digitalWrite(FRONT_TIBIA_L, LOW);
    }
    else                                   
    {                                     
        digitalWrite(FRONT_TIBIA_L, HIGH); 
        delayMicroseconds(usec_FTb_L);
        digitalWrite(FRONT_TIBIA_L, LOW);
    }

    // Hip  ********************************************************************
    if (usec_FHp_L == 0)                   
    {                                      
        if (temp_count_FHL != count_FHL)   
        {                                  
            temp_count_FHL += 1.0;
            value_FHL = usec_from_FHL + (int)(((float)temp_count_FHL/(float)count_FHL) * ((float)usec_to_FHL - (float)usec_from_FHL) + 0.5);
        }
        digitalWrite(FRONT_HIP_L, HIGH);
        delayMicroseconds(value_FHL);
        digitalWrite(FRONT_HIP_L, LOW);
    }
    else                                   
    {                                      
        digitalWrite(FRONT_HIP_L, HIGH); 
        delayMicroseconds(usec_FHp_L);
        digitalWrite(FRONT_HIP_L, LOW);
    }
    if (int_On) interrupts();
}

void back_Servos_L()
{
    if (int_On) noInterrupts();
    // Femur ********************************************************************
    if (usec_BFe_L == 0)                   // If this is true then Servo Gearbox movement will take count_FL * 20 milliseconds
    {                                      // going to the angle defined by usec_to_FL from usec_from_FL
        if (temp_count_BFL != count_BFL)   // Note that it is possible to define a speed that exceeds the full speed value
        {                                  // of the Servo Gearbox
            temp_count_BFL += 1.0;
            value_BFL = usec_from_BFL + (int)(((float)temp_count_BFL/(float)count_BFL) * ((float)usec_to_BFL - (float)usec_from_BFL) + 0.5);
        }
        digitalWrite(BACK_FEMUR_L, HIGH);
        delayMicroseconds(value_BFL);
        digitalWrite(BACK_FEMUR_L, LOW);
    }
    else                                   // Otherwise just move to the desired angle, using usec value, at full speed 
    {                                      // The SG12-50 travels 48 deg/sec at 4.8 volts or 75 deg/sec at 7.4 volts
        digitalWrite(BACK_FEMUR_L, HIGH); // The SG12-70 travels 34.29 deg/sec at 4.8 volts or 53.57 deg/sec at 7.4 volts
        delayMicroseconds(usec_BFe_L);
        digitalWrite(BACK_FEMUR_L, LOW);
    }

    // Tibia ********************************************************************
    if (usec_BTb_L == 0)                   
    {                                     
        if (temp_count_BTL != count_BTL)   
        {                                  
            temp_count_BTL += 1.0;
            value_BTL = usec_from_BTL + (int)(((float)temp_count_BTL/(float)count_BTL) * ((float)usec_to_BTL - (float)usec_from_BTL) + 0.5);
        }
        digitalWrite(BACK_TIBIA_L, HIGH);
        delayMicroseconds(value_BTL);
        digitalWrite(BACK_TIBIA_L, LOW);
    }
    else                                   
    {                                     
        digitalWrite(BACK_TIBIA_L, HIGH); 
        delayMicroseconds(usec_BTb_L);
        digitalWrite(BACK_TIBIA_L, LOW);
    }

    // Hip   ********************************************************************
    if (usec_BHp_L == 0)                   
    {                                      
        if (temp_count_BHL != count_BHL)   
        {                                  
            temp_count_BHL += 1.0;
            value_BHL = usec_from_BHL + (int)(((float)temp_count_BHL/(float)count_BHL) * ((float)usec_to_BHL - (float)usec_from_BHL) + 0.5);
        }
        digitalWrite(BACK_HIP_L, HIGH);
        delayMicroseconds(value_BHL);
        digitalWrite(BACK_HIP_L, LOW);
    }
    else                                   
    {                                      
        digitalWrite(BACK_HIP_L, HIGH); 
        delayMicroseconds(usec_BHp_L);
        digitalWrite(BACK_HIP_L, LOW);
    }
    if (int_On) interrupts();
}

void front_Servos_R()
{
    if (int_On) noInterrupts();
    // Femur ********************************************************************
    if (usec_FFe_R == 0)                   // If this is true then Servo Gearbox movement will take count_FL * 20 milliseconds
    {                                      // going to the angle defined by usec_to_FL from usec_from_FL
        if (temp_count_FFR != count_FFR)   // Note that it is possible to define a speed that exceeds the full speed value
        {                                  // of the Servo Gearbox
            temp_count_FFR += 1.0;
            value_FFR = usec_from_FFR + (int)(((float)temp_count_FFR/(float)count_FFR) * ((float)usec_to_FFR - (float)usec_from_FFR) + 0.5);
        }
        digitalWrite(FRONT_FEMUR_R, HIGH);
        delayMicroseconds(value_FFR);
        digitalWrite(FRONT_FEMUR_R, LOW);
    }
    else                                   // Otherwise just move to the desired angle, using usec value, at full speed 
    {                                      // The SG12-50 travels 48 deg/sec at 4.8 volts or 75 deg/sec at 7.4 volts
        digitalWrite(FRONT_FEMUR_R, HIGH); // The SG12-70 travels 34.29 deg/sec at 4.8 volts or 53.57 deg/sec at 7.4 volts
        delayMicroseconds(usec_FFe_R);
        digitalWrite(FRONT_FEMUR_R, LOW);
    }

    // Tibia ********************************************************************
    if (usec_FTb_R == 0)                   
    {                                     
        if (temp_count_FTR != count_FTR)   
        {                                  
            temp_count_FTR += 1.0;
            value_FTR = usec_from_FTR + (int)(((float)temp_count_FTR/(float)count_FTR) * ((float)usec_to_FTR - (float)usec_from_FTR) + 0.5);
        }
        digitalWrite(FRONT_TIBIA_R, HIGH);
        delayMicroseconds(value_FTR);
        digitalWrite(FRONT_TIBIA_R, LOW);
    }
    else                                   
    {                                     
        digitalWrite(FRONT_TIBIA_R, HIGH); 
        delayMicroseconds(usec_FTb_R);
        digitalWrite(FRONT_TIBIA_R, LOW);
    }

    // Hip   ********************************************************************
    if (usec_FHp_R == 0)                   
    {                                      
        if (temp_count_FHR != count_FHR)   
        {                                  
            temp_count_FHR += 1.0;
            value_FHR = usec_from_FHR + (int)(((float)temp_count_FHR/(float)count_FHR) * ((float)usec_to_FHR - (float)usec_from_FHR) + 0.5);
        }
        digitalWrite(FRONT_HIP_R, HIGH);
        delayMicroseconds(value_FHR);
        digitalWrite(FRONT_HIP_R, LOW);
    }
    else                                   
    {                                      
        digitalWrite(FRONT_HIP_R, HIGH); 
        delayMicroseconds(usec_FHp_R);
        digitalWrite(FRONT_HIP_R, LOW);
    }
    if (int_On) interrupts();
}

void back_Servos_R()
{
    if (int_On) noInterrupts();
    // Femur ********************************************************************
    if (usec_BFe_R == 0)                   // If this is true then Servo Gearbox movement will take count_FL * 20 milliseconds
    {                                      // going to the angle defined by usec_to_FL from usec_from_FL
        if (temp_count_BFR != count_BFR)   // Note that it is possible to define a speed that exceeds the full speed value
        {                                  // of the Servo Gearbox
            temp_count_BFR += 1.0;
            value_BFR = usec_from_BFR + (int)(((float)temp_count_BFR/(float)count_BFR) * ((float)usec_to_BFR - (float)usec_from_BFR) + 0.5);
        }
        digitalWrite(BACK_FEMUR_R, HIGH);
        delayMicroseconds(value_BFR);
        digitalWrite(BACK_FEMUR_R, LOW);
    }
    else                                   // Otherwise just move to the desired angle, using usec value, at full speed 
    {                                      // The SG12-50 travels 48 deg/sec at 4.8 volts or 75 deg/sec at 7.4 volts
        digitalWrite(BACK_FEMUR_R, HIGH); // The SG12-70 travels 34.29 deg/sec at 4.8 volts or 53.57 deg/sec at 7.4 volts
        delayMicroseconds(usec_BFe_R);
        digitalWrite(BACK_FEMUR_R, LOW);
    }

    // Tibia ********************************************************************
    if (usec_BTb_R == 0)                   
    {                                     
        if (temp_count_BTR != count_BTR)   
        {                                  
            temp_count_BTR += 1.0;
            value_BTR = usec_from_BTR + (int)(((float)temp_count_BTR/(float)count_BTR) * ((float)usec_to_BTR - (float)usec_from_BTR) + 0.5);
        }
        digitalWrite(BACK_TIBIA_R, HIGH);
        delayMicroseconds(value_BTR);
        digitalWrite(BACK_TIBIA_R, LOW);
    }
    else                                   
    {                                     
        digitalWrite(BACK_TIBIA_R, HIGH); 
        delayMicroseconds(usec_BTb_R);
        digitalWrite(BACK_TIBIA_R, LOW);
    }

    // Hip   ********************************************************************
    if (usec_BHp_R == 0)                   
    {                                      
        if (temp_count_BHR != count_BHR)   
        {                                  
            temp_count_BHR += 1.0;
            value_BHR = usec_from_BHR + (int)(((float)temp_count_BHR/(float)count_BHR) * ((float)usec_to_BHR - (float)usec_from_BHR) + 0.5);
        }
        digitalWrite(BACK_HIP_R, HIGH);
        delayMicroseconds(value_BHR);
        digitalWrite(BACK_HIP_R, LOW);
    }
    else                                   
    {                                      
        digitalWrite(BACK_HIP_R, HIGH); 
        delayMicroseconds(usec_BHp_R);
        digitalWrite(BACK_HIP_R, LOW);
    }
    if (int_On) interrupts();
}

// ******************************************************************************
// Reads incoming serial from ESP32, 2 character command
void mySerial_Event() 
{
    while ((Serial2.available() > 0) && (stringComplete == false))
    {
        // get the new char
        char inChar = (char)Serial2.read();
      //  Serial.println(inChar);
       
        // if the incoming character is a newline, set a flag
        // so the main loop can do something about it:
        if (inChar == '\n') 
        {
          stringComplete = true;
          inputString = inputString.substring(0,2);
          Serial.println(inputString);
          break;
        }
        // add it to the inputString:
        inputString += inChar;
    }
}

// ******************************************************************************
// Reads Nextion command and stores value

void get_Master_command()
{
//    Serial.print("****");
//    Serial.print(inputString);
//    Serial.println("****");

    if (inputString == "ST")    // STOP
    {
        Serial.println("STOP");
        digitalWrite(LED, HIGH);
        all_stop = true;
    }
    if (inputString == "UP")    // Stand Up
    {
        Serial.println("Stand Up");
        //stand_up();
    }
    if (inputString == "SD")    // Sit Down
    {
         Serial.println("Sit Down");
    }
    if (inputString == "WA")    // Walk
    {
         Serial.println("Walk");
    }
    if (inputString == "WS")    // Walk Slow
    {
         Serial.println("Walk Slow");
    }
    if (inputString == "OS")    // One Step
    {
         //oneStep();
         Serial.println("One Step");
    }
    if (inputString == "TE")    // Test
    {
         //myTest();
         Serial.println("Test");
    }
    if (inputString == "RE")    // Resume
    {
         Serial.println("Resume");
         all_stop = false;
         digitalWrite(LED, LOW);
    }
}

// ******************************************************************************
// Stand Up from sitting position
// how = 0 = both front and back together, how = 1 = back first then front
void stand_Up(int how)
{
    // Stand Up
//    for (int i = 1; i <= 24; i++)
//    { 
//        moveToAngle(65.0 - (float)i * 5.0 / 24.0, 0.0, BACK_FEMUR_L);
//        moveToAngle(65.0 - (float)i * 5.0 / 24.0, 0.0, BACK_FEMUR_R);
//        moveToAngle((float)i * 2.23, 65.0 - (float)i * 5.0 / 24.0, BACK_TIBIA_L);
//        moveToAngle((float)i * 2.23, 65.0 - (float)i * 5.0 / 24.0, BACK_TIBIA_R);
//        delay(150);
//    }

//    for (int i = 1; i <= 24; i++)
//    {
//        moveToAngle(65.0 - (float)i * 5.0 / 24.0, 0.0, FRONT_FEMUR_L);
//        moveToAngle(65.0 - (float)i * 5.0 / 24.0, 0.0, FRONT_FEMUR_R);
//        moveToAngle((float)i * 2.23, 65.0 - (float)i * 5.0 / 24.0, FRONT_TIBIA_L);
//        moveToAngle((float)i * 2.23, 65.0 - (float)i * 5.0 / 24.0, FRONT_TIBIA_R);
//        delay(150);
//    }

        if (how == 0)
        {
            the_count = 100;
            usec_from_FFL = FRONT_femur_L_uSec(65.0);   // Get microseconds needed for
            usec_to_FFL = FRONT_femur_L_uSec(60.0);     // from and to angles
            count_FFL = the_count;                      // Set count_FL, number of 20 millisecond periods for speed
            temp_count_FFL = 0;                         // Zero the temporary counter
            usec_FFe_L = 0;                             // Set usec_Fe_L = 0, so speed will be used
      
            usec_from_FTL = FRONT_tibia_L_uSec(0.0);
            usec_to_FTL = FRONT_tibia_L_uSec(53.5);
            count_FTL = the_count;
            temp_count_FTL = 0;
            usec_FTb_L = 0;
      
            usec_from_FFR = FRONT_femur_R_uSec(65.0);
            usec_to_FFR = FRONT_femur_R_uSec(60.0);
            count_FFR = the_count;
            temp_count_FFR = 0;
            usec_FFe_R = 0;
      
            usec_from_FTR = FRONT_tibia_R_uSec(0.0);
            usec_to_FTR = FRONT_tibia_R_uSec(53.5);
            count_FTR = the_count;
            temp_count_FTR = 0;
            usec_FTb_R = 0;

            usec_from_BFL = BACK_femur_L_uSec(65.0);
            usec_to_BFL = BACK_femur_L_uSec(60.0);
            count_BFL = the_count;
            temp_count_BFL = 0;
            usec_BFe_L = 0;
      
            usec_from_BTL = BACK_tibia_L_uSec(0.0);
            usec_to_BTL = BACK_tibia_L_uSec(53.5);
            count_BTL = the_count;
            temp_count_BTL = 0;
            usec_BTb_L = 0;
      
            usec_from_BFR = BACK_femur_R_uSec(65.0);
            usec_to_BFR = BACK_femur_R_uSec(60.0);
            count_BFR = the_count;
            temp_count_BFR = 0;
            usec_BFe_R = 0;
      
            usec_from_BTR = BACK_tibia_R_uSec(0.0);
            usec_to_BTR = BACK_tibia_R_uSec(53.5);
            count_BTR = the_count;
            temp_count_BTR = 0;
            usec_BTb_R = 0;


            while (temp_count_BTR != count_BTR)
            {
              // just wait
            }
          //  return;
        }
        else
        {
          the_count = 100;
          usec_from_BFL = BACK_femur_L_uSec(65.0);
          usec_to_BFL = BACK_femur_L_uSec(60.0);
          count_BFL = the_count;
          temp_count_BFL = 0;
          usec_BFe_L = 0;
    
          usec_from_BTL = BACK_tibia_L_uSec(0.0);
          usec_to_BTL = BACK_tibia_L_uSec(53.5);
          count_BTL = the_count;
          temp_count_BTL = 0;
          usec_BTb_L = 0;
    
          usec_from_BFR = BACK_femur_R_uSec(65.0);
          usec_to_BFR = BACK_femur_R_uSec(60.0);
          count_BFR = the_count;
          temp_count_BFR = 0;
          usec_BFe_R = 0;
    
          usec_from_BTR = BACK_tibia_R_uSec(0.0);
          usec_to_BTR = BACK_tibia_R_uSec(53.5);
          count_BTR = the_count;
          temp_count_BTR = 0;
          usec_BTb_R = 0;

          while (temp_count_BTR != count_BTR)
          {
            // just wait
          }

          usec_from_FFL = FRONT_femur_L_uSec(65.0);   // Get microseconds needed for
          usec_to_FFL = FRONT_femur_L_uSec(60.0);     // from and to angles
          count_FFL = the_count;                      // Set count_FL, number of 20 millisecond periods for speed
          temp_count_FFL = 0;                         // Zero the temporary counter
          usec_FFe_L = 0;                             // Set usec_Fe_L = 0, so speed will be used
    
          usec_from_FTL = FRONT_tibia_L_uSec(0.0);
          usec_to_FTL = FRONT_tibia_L_uSec(53.5);
          count_FTL = the_count;
          temp_count_FTL = 0;
          usec_FTb_L = 0;
    
          usec_from_FFR = FRONT_femur_R_uSec(65.0);
          usec_to_FFR = FRONT_femur_R_uSec(60.0);
          count_FFR = the_count;
          temp_count_FFR = 0;
          usec_FFe_R = 0;
    
          usec_from_FTR = FRONT_tibia_R_uSec(0.0);
          usec_to_FTR = FRONT_tibia_R_uSec(53.5);
          count_FTR = the_count;
          temp_count_FTR = 0;
          usec_FTb_R = 0;

           while (temp_count_FTR != count_FTR)
          {
            // just wait
          }
        }
}