/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-17 https://www.robotway.com/

  ------------------------------*/

#include <Tlc5940.h>

#include <tlc_servos.h>

#define ANGLE_VALUE_MIN 0

#define ANGLE_VALUE_MAX 180

#define PWM_VALUE_MIN 500

#define PWM_VALUE_MAX 2500

int data_array[2] = {0,0};   //servo_pin: data_array[0], servo_value: data_array[1];

String data = "";

boolean dataComplete = false;

void setup() {

  Serial.begin(9600);

  Tlc.init(0);

  tlc_initServos();

  delay(1000);

}

void loop() {

  while(Serial.available())

  {

    int B_flag, P_flag, T_flag;

    data = Serial.readStringUntil('\n');

    data.trim();

    for(int i=0;i<data.length();i++)

    {

      //Serial.println(data[i]);

      switch(data[i])

      {

        case '#':

          B_flag = i;  

        break;

        case 'P':

        {

          String pin = "";

          P_flag = i;

          for(int i=B_flag+1;i<P_flag;i++)

          {

            pin+=data[i];

          }

          data_array[0] = pin.toInt();

        }

        break;

        case 'T':

        {

          String angle = "";

          T_flag = i;

          for(int i=P_flag+1;i<T_flag;i++)

          {

            angle += data[i];

          }

          data_array[1] = angle.toInt();

        }

        break;

        default: break;

      }     

    }

    /*

    Serial.println(B_flag);

    Serial.println(P_flag);

    Serial.println(T_flag);

    for(int i=0;i<2;i++)

    {

      Serial.println(data_array[i]);

    }

    */

    dataComplete = true;

  }

  if(dataComplete)

  {

    if(data_array[1] >= ANGLE_VALUE_MIN && data_array[1] <= ANGLE_VALUE_MAX)

    {

      tlc_setServo(data_array[0], data_array[1]);

    }

    else if(data_array[1] >= PWM_VALUE_MIN && data_array[1] <= PWM_VALUE_MAX)

    {

      data_array[1] = map(data_array[1], 500, 2500, 0, 180);

      tlc_setServo(data_array[0], data_array[1]);

    }

    Tlc.update();

    dataComplete = false;

  }

}

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-17 https://www.robotway.com/

  ------------------------------*/

/*

* Bigfish扩展板舵机口; 4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19

* 使用软件调节舵机时请拖拽对应序号的控制块

*/

#include <Servo.h>

#define ANGLE_VALUE_MIN 0

#define ANGLE_VALUE_MAX 180

#define PWM_VALUE_MIN 500

#define PWM_VALUE_MAX 2500

#define SERVO_NUM 12

Servo myServo[SERVO_NUM];

int data_array[2] = {0,0};   //servo_pin: data_array[0], servo_value: data_array[1];

int servo_port[SERVO_NUM] = {4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19};

int servo_value[SERVO_NUM] = {};

String data = "";

boolean dataComplete = false;

void setup() {

  Serial.begin(9600);

}

void loop() {

  while(Serial.available())

  {

    int B_flag, P_flag, T_flag;

    data = Serial.readStringUntil('\n');

    data.trim();

    for(int i=0;i<data.length();i++)

    {

      //Serial.println(data[i]);

      switch(data[i])

      {

        case '#':

          B_flag = i;  

        break;

        case 'P':

        {

          String pin = "";

          P_flag = i;

          for(int i=B_flag+1;i<P_flag;i++)

          {

            pin+=data[i];

          }

          data_array[0] = pin.toInt();

        }

        break;

        case 'T':

        {

          String angle = "";

          T_flag = i;

          for(int i=P_flag+1;i<T_flag;i++)

          {

            angle += data[i];

          }

          data_array[1] = angle.toInt();

          servo_value[pin2index(data_array[0])] = data_array[1];

        }

        break;

        default: break;

      }     

    }

    /*

    Serial.println(B_flag);

    Serial.println(P_flag);

    Serial.println(T_flag);

    for(int i=0;i<2;i++)

    {

      Serial.println(data_array[i]);

    }

    */

    dataComplete = true;

  }

  if(dataComplete)

  {

    for(int i=0;i<SERVO_NUM;i++)

    {

      ServoGo(i, servo_value[i]);

      /*********************************串口查看输出***********************************/

//      Serial.print(servo_value[i]);

//      Serial.print(" ");

    }

//    Serial.println();

      /*********************************++++++++++++***********************************/

    dataComplete = false;

  }

}

void ServoStart(int which){

  if(!myServo[which].attached() && (servo_value[which] != 0))myServo[which].attach(servo_port[which]);

      else return;

  pinMode(servo_port[which], OUTPUT);

}

void ServoStop(int which){

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , int where){

  ServoStart(which);

  if(where >= ANGLE_VALUE_MIN && where <= ANGLE_VALUE_MAX)

  {

    myServo[which].write(where);

  }

  else if(where >= PWM_VALUE_MIN && where <= PWM_VALUE_MAX)

  {

    myServo[which].writeMicroseconds(where);

  }

}

int pin2index(int _pin){

  int index;

  switch(_pin)

  {

    case 4: index = 0; break;

    case 7: index = 1; break;

    case 11: index = 2; break;

    case 3: index = 3; break;

    case 8: index = 4; break;

    case 12: index = 5; break;

    case 14: index = 6; break;

    case 15: index = 7; break;

    case 16: index = 8; break;

    case 17: index = 9; break;

    case 18: index = 10; break;

    case 19: index = 11; break;

  }

  return index;

}