/*******************************************************************************
* Uses code lines copied from example "velocity_mode" of the library Dynamixel2Arduino
* Copyright 2016 ROBOTIS CO., LTD.
* Licensed under the Apache License, Version 2.0 (the "License");
* Adapted in Feb 2026 By G. Morel for the POLYTECH ROB3 PROJECT
* In this example, you have code examples to control a KTECH motor, a DYNAMIXEL motor
* (model compatible with protocol 1.0 such as MX-28), and a servo motot controlling a gripper
*******************************************************************************/
#include <math.h>




// Libraries for KT motors
// Arduino library for communacting with SPI devices
#include <SPI.h>
// CAN communications
#include <mcp2515_can.h>
const int SPI_CS_PIN = 9;
mcp2515_can CAN(SPI_CS_PIN);  // Creates the mcp2515_can class object for CAN communication, while setting CS pin
// Homemade KTmotor interface
#include "Ktech_motor.h"
// Declaration of the two motors with their ID
#define NUMBER_OF_MOTORS 2
Motor KTMOTORS[NUMBER_OF_MOTORS] = { Motor(2), Motor(3) };  // 1 and 2 are the motor IDs for CAN protocol 



// Libraries for DXL motor
#include <Dynamixel2Arduino.h>
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8);  // PIN NUMBER FOR RX/TX UART COMMUNICATION WITH YOUR PC
#define PC_SERIAL soft_serial
#define DXL_SERIAL Serial
const int DXL_DIR_PIN = 2;  // DYNAMIXEL Shield DIR PIN
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 1.0;
// creates dxl as the dynamixel object
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
//This namespace is required to use Control table item names
using namespace ControlTableItem;




// Gripper PIN
#define GRIPPER_PIN 5




// Constant definitions
#if !defined(FALSE)
#define FALSE 0
#define TRUE 1
#endif
#define MY_PI 3.14159265359




#define SEND_A_POSITION_TO_DXL TRUE
#define SEND_A_VELOCITY_TO_KTM TRUE
#define MEASURE_THE_US_SENSORS TRUE
#define MOVE_THE_GRIPPER TRUE
#define PRINTING_ON TRUE




// Global variables
// Variables for US sensors
#define NUMBER_OF_US_SENSORS 2
const int trigPinUS[NUMBER_OF_US_SENSORS] = { 22, 42 };
const int echoPinUS[NUMBER_OF_US_SENSORS] = { 23, 43 };
float durationUS[NUMBER_OF_US_SENSORS] = { 0.0, 0.0 };
float distanceUS[NUMBER_OF_US_SENSORS] = { 0.0, 0.0 };
int etat = 0;
float vit_mot = 80;
float vit_mot_tourne = 50;
float pos_deg_D_init = -1.0;
float pos_deg_G_init = -1.0;
int comp = 0;
float motorVelocityCommandInDegPerSec = 0.0;
float long_pincecapt=6;
float dist_a1 = 0;
float a0 = 0.0;
float Kp = 2.5;
float corr_max = 20.0;
















// Clock
unsigned long currentTimeInMicros = 0;
float currentTimeInS = 0.0;




// Fonctions
void mes_US() {
 for (int i = 0; i < NUMBER_OF_US_SENSORS; i++) {
   digitalWrite(trigPinUS[i], LOW);
   delayMicroseconds(2);
   digitalWrite(trigPinUS[i], HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPinUS[i], LOW);
   durationUS[i] = pulseIn(echoPinUS[i], HIGH, 30000);
   distanceUS[i] = (durationUS[i] * .0343) / 2;
 }
}




int tourn_90hor() {
  float L = 0.2;   
  float D = 0.05;  
  //float X = 90.0 * L / D;
  float X = 86.0 * L / D;


  if (pos_deg_G_init == -1.0) {  
    KTMOTORS[0].sendVelocityCommand(0, CAN);
    KTMOTORS[1].sendVelocityCommand(0, CAN);
    pos_deg_G_init = KTMOTORS[0].prevPosInDeg;
    
    PC_SERIAL.print("--- Rotation 90 Hor - Pos Init G: ");
    PC_SERIAL.println(pos_deg_G_init);
    return 0;
  } else {
    KTMOTORS[0].sendVelocityCommand((long int)(vit_mot_tourne), CAN);
    KTMOTORS[1].sendVelocityCommand((long int)(vit_mot_tourne), CAN);

    float pos_deg_G_act = KTMOTORS[0].prevPosInDeg;
    float deplacement = fabs(pos_deg_G_act - pos_deg_G_init);
    
    if (deplacement >= X) {  
      KTMOTORS[0].sendVelocityCommand(0, CAN);
      KTMOTORS[1].sendVelocityCommand(0, CAN);
      pos_deg_G_init = -1.0; 
      return 1; 
    }
    return 0; 
  }
}






int tourn_90antihor() {
 float L = 0.2;   
 float D = 0.05;  
 //float X = 90 * L / D; 
 float X = 86 * L / D; 

 
 if (pos_deg_D_init == -1.0) {
   KTMOTORS[0].sendVelocityCommand(0, CAN); 
   KTMOTORS[1].sendVelocityCommand(0, CAN); 

   pos_deg_D_init = KTMOTORS[1].prevPosInDeg;
   PC_SERIAL.println(pos_deg_D_init);
   return 0;
 } else {
   motorVelocityCommandInDegPerSec = vit_mot_tourne;
   

   KTMOTORS[0].sendVelocityCommand((long int)(-motorVelocityCommandInDegPerSec), CAN);

   KTMOTORS[1].sendVelocityCommand((long int)(-motorVelocityCommandInDegPerSec), CAN);

   

   float pos_deg_D_act = KTMOTORS[1].prevPosInDeg;
   

   if (fabs(pos_deg_D_act - pos_deg_D_init) >= X) {
     motorVelocityCommandInDegPerSec = 0;
     KTMOTORS[0].sendVelocityCommand((long int)(motorVelocityCommandInDegPerSec), CAN);
     KTMOTORS[1].sendVelocityCommand((long int)(motorVelocityCommandInDegPerSec), CAN);
     pos_deg_D_init = -1.0;
     return 1;
   }
   return 0;
 }
}








int mot_avance(float dist_stop, float dist_mur_cote) {
 mes_US();
 float erreur = distanceUS[1] - dist_mur_cote;  
 float tolerance_lat = 0.5;
 
 if (distanceUS[0] <= dist_stop && distanceUS[0] > 5) {
   KTMOTORS[0].sendVelocityCommand(0, CAN);
   KTMOTORS[1].sendVelocityCommand(0, CAN);
   return 1;
 }
 
 float vG = vit_mot;
 float vD = vit_mot;
 
 if (fabs(erreur) > tolerance_lat) {
   float correction = erreur * Kp;
   if (correction > corr_max) correction = corr_max;
   if (correction < -corr_max) correction = -corr_max;
   
   vG = vit_mot + correction;
   vD = vit_mot - correction;
 }
 KTMOTORS[0].sendVelocityCommand((long)vG, CAN);
 delay(2);
 KTMOTORS[1].sendVelocityCommand(-(long)vD, CAN);
  delay(10);

 return 0;
}











void leve_totem() {
 float angle_cible = 130.0;
 for(float angle = 60.0; angle < angle_cible; angle++){
  dxl.setGoalPosition(DXL_ID, angle, UNIT_DEGREE);
  delay(100);
 }
}

void baisse_totem() {
 float angle_cible = 60.0;
 for(float angle = 130.0; angle > angle_cible; angle--){
  dxl.setGoalPosition(DXL_ID, angle, UNIT_DEGREE);
  delay(100);
 }
}

void gerer_pince(int val) {
 analogWrite(GRIPPER_PIN, val);
 delay(100);
}








void setup() {
 int i;
dxl.setGoalPosition(DXL_ID, 130, UNIT_DEGREE);

 // Setup digital I/O for US sensor
 for (i = 0; i < NUMBER_OF_US_SENSORS; i++) {
   pinMode(trigPinUS[i], OUTPUT);
   pinMode(echoPinUS[i], INPUT);
 }




 // Use UART port of DYNAMIXEL Shield to debug.
 PC_SERIAL.begin(9600);




 // Initialization of the CAN communication. THis will wait until the motor is powered on
 while (CAN_OK != CAN.begin(CAN_500KBPS)) {
   PC_SERIAL.println("CAN init fail, retry ...");
   delay(500);
 }




 // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
 dxl.begin(1000000);
 // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
 dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 // Get DYNAMIXEL information
 dxl.ping(DXL_ID);
 // Turn off torque when configuring items in EEPROM area
 dxl.torqueOff(DXL_ID);
 dxl.setOperatingMode(DXL_ID, OP_POSITION);
 dxl.torqueOn(DXL_ID);
 // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
 dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 5);




 // Initialization of the motor command and the position measurement variables
 KTMOTORS[0].prevPosInDeg = 0.0;
 KTMOTORS[0].revolNumber = 0;
 KTMOTORS[0].offsetEncoder = 0;
 // Send motor off then motor on command to reset
 KTMOTORS[0].OFF(CAN);

 delay(10);



 KTMOTORS[0].ON(CAN);
 delay(10);
 KTMOTORS[0].sendVelocityCommand((long int)(0), CAN);
  delay(10);
 KTMOTORS[0].revolNumber = 0;
 KTMOTORS[0].prevPosInDeg = 0.0;




 // Initialization of the motor command and the position measurement variables
 KTMOTORS[1].prevPosInDeg = 0.0;
 KTMOTORS[1].revolNumber = 0;
 KTMOTORS[1].offsetEncoder = 0;
 // Send motor off then motor on command to reset
 KTMOTORS[1].OFF(CAN);
  delay(10);

 KTMOTORS[1].ON(CAN);
  delay(10);

 KTMOTORS[1].sendVelocityCommand((long int)(0), CAN);
  delay(10);

 KTMOTORS[1].revolNumber = 0;
 KTMOTORS[1].prevPosInDeg = 0.0;












 PC_SERIAL.println("");
 PC_SERIAL.println("");
 PC_SERIAL.println("************************************************************");
 PC_SERIAL.println(" LE SYSTEME EST PRET A FONCTIONNER. SIGNAL DE COMMANDE = 0 *");
 PC_SERIAL.println("************************************************************");
 PC_SERIAL.println("");

 delay(1000);
}




#define SWITCH_PERIOD 4
int firstTimeInHalfPeriod1 = TRUE;
int firstTimeInHalfPeriod2 = TRUE;




void loop() {
 
 int i;
 float dxlPositionInDegrees;
 currentTimeInMicros = micros();
 currentTimeInS = 0.000001 * ((float)currentTimeInMicros);
 static int etat_precedent = -1;
 if (etat != etat_precedent) {
   PC_SERIAL.print("--- PASSAGE A L'ETAT : ");
   PC_SERIAL.println(etat);
   etat_precedent = etat;
 }
 switch (etat) {
   case 0:
     mes_US();
     a0 = distanceUS[1];
     PC_SERIAL.print("Distance init mur (a0) : ");
     PC_SERIAL.println(a0);
     delay(5000);
     dxl.setGoalPosition(DXL_ID, 130.0, UNIT_DEGREE);
     etat = 1;
     break;




   case 1:  

    static unsigned long t_start = 0;

    if (t_start == 0) {
        t_start = millis();
    }

    KTMOTORS[0].sendVelocityCommand(vit_mot, CAN);
    KTMOTORS[1].sendVelocityCommand(-vit_mot, CAN);

    if (millis() - t_start >= 6500) {

        KTMOTORS[0].sendVelocityCommand(0, CAN);
        KTMOTORS[1].sendVelocityCommand(0, CAN);

        t_start = 0;
        etat = 2;
    }
     break;





   case 2:  //tourne de 90° dans le sens horaire
  

     comp = tourn_90hor();
     if (comp == 1) {
       mes_US();
       a0 = distanceUS[1];
       etat = 3;
     }
     break;




   case 3:  
    mes_US();

    // Avance tout droit
    KTMOTORS[0].sendVelocityCommand(vit_mot, CAN);
    KTMOTORS[1].sendVelocityCommand(-vit_mot, CAN);

    // Condition d'arrêt
    if (distanceUS[0] <= 8 && distanceUS[0] > 5) {

        KTMOTORS[0].sendVelocityCommand(0, CAN);
        KTMOTORS[1].sendVelocityCommand(0, CAN);

        etat = 4;
    }
     break;




   case 4:  //tourne de 90° dans le sens anti-horaire
     comp = tourn_90antihor();
     if (comp == 1) {
        dxl.setGoalPosition(DXL_ID, 64 , UNIT_DEGREE);
       etat = 5;
     }
     break;




   case 5:  //avance jusqu'au totem et ferme la pince
     //comp = mot_avance(long_pincecapt,22);
     comp = mot_avance(long_pincecapt,24.5);

     if (comp == 1) {
       delay(2000); 
       etat = 6;
     }
     break;




   case 6:  //soulève la pince
     PC_SERIAL.println("Avant");
     KTMOTORS[0].sendVelocityCommand(0, CAN);
     KTMOTORS[1].sendVelocityCommand(0, CAN);
     delay(2000); 
     leve_totem();
     delay(2000);
     PC_SERIAL.println("Après");
     etat = 7;
     break;




   case 7:  //récupère a1
     mes_US();
     if(distanceUS[0]>13){
      dist_a1 = distanceUS[0];
     
      PC_SERIAL.println(dist_a1);
      etat = 8;
     }
     break;




   case 8:  //tourne de 90° dans le sens anti-horaire
     comp = tourn_90antihor();
     if (comp == 1) {
       mes_US(); 
       a0 = distanceUS[1]; 
       etat = 9;
     }
     break;




   case 9:  
     mes_US();

    // Avance tout droit
    KTMOTORS[0].sendVelocityCommand(vit_mot, CAN);
    KTMOTORS[1].sendVelocityCommand(-vit_mot, CAN);

    // Condition d'arrêt
    if (distanceUS[0] <= 13 && distanceUS[0] > 5) {

        KTMOTORS[0].sendVelocityCommand(0, CAN);
        KTMOTORS[1].sendVelocityCommand(0, CAN);

        etat = 10;
    }
     break;




   case 10:  //tourne de 90° dans le sens anti-horaire
     comp = tourn_90antihor();
     if (comp == 1) {
       mes_US();
       a0 = distanceUS[1];
       etat = 11;
     }
     break;


   case 11:  //avance jusqu'à a1 du mur
     comp = mot_avance(dist_a1, 24.5);
     if (comp == 1) {
       etat = 12;
     }
     break;




   case 12:  //reposer le totem
    KTMOTORS[0].sendVelocityCommand(0, CAN);
    KTMOTORS[1].sendVelocityCommand(0, CAN);
     baisse_totem();
     etat = 13;
     break;

   case 13: //reculer et stopper
    static unsigned long t_start2 = 0;

    if (t_start2 == 0) {
        t_start2 = millis();
    }

    KTMOTORS[0].sendVelocityCommand(-vit_mot, CAN);
    KTMOTORS[1].sendVelocityCommand(vit_mot, CAN);

    if (millis() - t_start2 >= 5000) {

        KTMOTORS[0].sendVelocityCommand(0, CAN);
        KTMOTORS[1].sendVelocityCommand(0, CAN);

        t_start2 = 0;
        etat = 14;
    }
    break;

    case 14:
      while (1);



 }
}