#include <SCServo.h>

#define SERVO_FOUCHE 5
#define SERVO_PINCE_GAUCHE 8
#define SERVO_PINCE_DROITE 9

#define FOURCHE_TRANSPORT 5, 307
#define FOURCHE_LEVEE     5, 227
#define FOURCHE_PRISE     5, 345

SMS_STS sms_sts;

int ID_Feetech = 4;

struct position_t{
  int nb_tour;
  uint position;
} cible_haute, cible_basse, actuelle;

void Moteur_set(int vitesse){
  ledcWrite(3, 0);
  ledcWrite(4, 0);
}

void setup()
{
  Serial.begin(115200);//sms舵机波特率115200
  Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
  sms_sts.pSerial = &Serial1;
  delay(1000);
  sms_sts.WheelMode(ID_Feetech);//舵机ID1切换至电机恒速模式
  //sms_sts.WriteSpe(ID_Feetech, 2400, 100);
  cible_haute.nb_tour = 0;
  cible_haute.position = 500;

  cible_basse.nb_tour = 2;
  cible_basse.position = 1500;

  actuelle.nb_tour = 0;

  //Servo fourche (12V)
  ledcAttach(5, 50, 12);
  // ledcWrite(5, 307); // Neutre
  ledcWrite(5, 307); // Position de transport
  //ledcWrite(5, 227); // Position levée (butée haute)
  //ledcWrite(5, 345); // Position de prise


  while(1){
    ledcWrite(FOURCHE_PRISE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(1000);
    ledcWrite(FOURCHE_LEVEE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(5000);

  }


  // Servo pinces à aimant
  ledcAttach(8, 50, 12);
  ledcWrite(8, 307);

  ledcAttach(9, 50, 12);
  ledcWrite(9, 307);

  // Moteur
  ledcAttach(3, 2000, 8);
  ledcWrite(3, 0);
  ledcAttach(4, 2000, 8);
  ledcWrite(4, 0);


  if(sms_sts.FeedBack(ID_Feetech)!=-1){
    actuelle.position = sms_sts.ReadPos(-1);
  }else{
    do{
      Serial.println("Erreur lecture");
      delay(1000);
    }while(sms_sts.FeedBack(ID_Feetech) ==-1);
    actuelle.position = sms_sts.ReadPos(-1);
  }

  
}

void goto_position(int servo_id, position_t position){
  // Actualise la position
  struct position_t precedente;
  static int loin = 1;
  precedente = actuelle;
  if(sms_sts.FeedBack(servo_id)!=-1){
    actuelle.position = sms_sts.ReadPos(-1);
  }
  if(abs((int)actuelle.position - (int)precedente.position) > 2000){
    // Rebouclage compteur
    if(actuelle.position < precedente.position){
      // Sens positif
      actuelle.nb_tour++;
    }else{
      actuelle.nb_tour--;
    }
  }

  // Sens de déplacement
  if(actuelle.nb_tour != position.nb_tour){
    if(loin != 1){
      loin = 1;
      sms_sts.WheelMode(servo_id);
      if(actuelle.nb_tour < position.nb_tour){
        sms_sts.WriteSpe(servo_id, 2400, 50);
      }else{
        sms_sts.WriteSpe(servo_id, -2400, 50);
      }
    }
  }else{
    if(loin != 0){
      loin = 0;
      sms_sts.ServoMode(servo_id);
      sms_sts.WritePosEx(ID_Feetech, position.position, 2400, 50);
    }
  }
  Serial.printf(">cible_pos:%d\n",position.position);
  Serial.printf(">cible_nb_tour:%d\n",position.nb_tour);
  Serial.printf(">act_pos:%d\n",actuelle.position);
  Serial.printf(">act_nb_tour:%d\n",actuelle.nb_tour);

}

void loop()
{
  static int m_pos=0;
  static unsigned long myTime=0;
  static position_t cible;

  /*
  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P1=4095位置
  sms_sts.WritePosEx(1, 4095, 2400, 50);
  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)

  //舵机(ID1)以最高速度V=60*0.732=43.92rpm，加速度A=50*8.7deg/s^2，运行至P0=0位置
  sms_sts.WritePosEx(1, 0, 2400, 50);
  delay((4095-0)*1000/(60*50) + (60*50)*10/(50) + 50);//[(P1-P0)/(V*50)]*1000+[(V*50)/(A*100)]*1000 + 50(误差)
  */
  Serial.print(">millis:");
  Serial.println(millis());
  if(millis() > myTime + 8000){
    myTime = millis();
    if(m_pos){
      //sms_sts.WritePosEx(ID_Feetech, 4095, 2400, 50);
      //sms_sts.WriteSpe(ID_Feetech, 2400, 50);
      cible = cible_haute;
      m_pos = 0;
    }else{
      m_pos ++;
      //sms_sts.WriteSpe(ID_Feetech, -2400, 50);
      //sms_sts.WritePosEx(ID_Feetech, 0, 2400, 50);
      //sms_sts.WriteSpe(ID_Feetech, -80, 100);
      cible = cible_basse;
    }
  }
  goto_position(ID_Feetech, cible);
  int ID = sms_sts.Ping(ID_Feetech);
  if(ID!=-1){
    Serial.print("Servo ID:");
    Serial.println(ID, DEC);
    delay(100);
  }else{
    Serial.println("Ping servo ID error!");
    delay(2000);
  }

    int Pos;
  int Speed;
  int Load;
  int Voltage;
  int Temper;
  int Move;
  int Current;
  if(sms_sts.FeedBack(ID)!=-1){
    Pos = sms_sts.ReadPos(-1);
    Speed = sms_sts.ReadSpeed(-1);
    Load = sms_sts.ReadLoad(-1);
    Voltage = sms_sts.ReadVoltage(-1);
    Temper = sms_sts.ReadTemper(-1);
    Move = sms_sts.ReadMove(-1);
    Current = sms_sts.ReadCurrent(-1);
    Serial.print(">Position:");
    Serial.println(Pos);
    Serial.print(">Speed:");
    Serial.println(Speed);
    Serial.print(">Load:");
    Serial.println(Load);
    Serial.print(">Voltage:");
    Serial.println(Voltage);
    Serial.print(">Temper:");
    Serial.println(Temper);
    Serial.print(">Move:");
    Serial.println(Move);
    Serial.print(">Current:");
    Serial.println(Current);
    delay(50);
  }else{
    Serial.println("FeedBack err");
    delay(500);
  }
}