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

  版权说明：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-04-20 https://www.robotway.com/

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

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

  实验功能：

    八自由度四足机器人运动

  实验接线：

                【--机器人头部(俯视图)--】

----------------------------------------------------

    左前腿[外侧腿----内侧腿]     右前腿[内侧腿----外侧腿]

            内侧    外侧                 内侧     外侧

             .--.      .--.                    .--.       .--.

             |   |---|   |                     |   |       |   |

    D3    |   |---|   |   D4        D7 |   |       |   | D8

             ---*   ---*                    *--*     *--*

    左后腿[外侧腿----内侧腿]     右后腿[内侧腿----外侧腿]

           内侧    外侧                  内侧     外侧

            .--.      .--.                    .--.       .--.

            |   |---|   |                     |   |      |   |

    A3   |   |---|   |   A2       D12 |   |      |   | D11

            ---*   ---*                    *--*    *--*

    ---------------------------------------------------

  版权说明：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-04-20 https://www.robotway.com/

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

#include <Arduino.h>

#include <avr/pgmspace.h>

#include <Servo.h>

#include "Config.h"

#include "PROGMEM_DATA.h"

Servo myServo[8];

void act_length();      //动作数组长度计算

void ServoStart();      //舵机连接

void ServoStop();       //舵机断开

void ServoGo();         //舵机转动

void readProgmem();     //读取PWM值

void servo_init();      //舵机初始化

void servo_move();      //动作执行

void setup() {

  Serial.begin(9600);

  act_length();             

}

void loop() {

  servo_move(ACTION_INIT, 2);

  delay(1000);

  servo_move(ACTION_MOVE, 20);

  while(1){};

}

void act_length()

{

  actPwmNum[0] = (sizeof(actionInit) / sizeof(actionInit[0]))/SERVO_NUM;

  actPwmNum[1] = (sizeof(actionMove) / sizeof(actionMove[0]))/SERVO_NUM;

  actPwmNum[2] = (sizeof(actionBack) / sizeof(actionBack[0]))/SERVO_NUM;

  actPwmNum[3] = (sizeof(actionLeft) / sizeof(actionLeft[0]))/SERVO_NUM;

  actPwmNum[4] = (sizeof(actionRight) / sizeof(actionRight[0]))/SERVO_NUM;

  /*******************+++++++++此处可以添加PWM数组++++++++++++****************/

}

void ServoStart(int which){

  if(!myServo[which].attached())myServo[which].attach(servo_port[which]);

  pinMode(servo_port[which], OUTPUT);

}

void ServoStop(int which){

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , float where){

  ServoStart(which);

  myServo[which].writeMicroseconds(where);

}

void readProgmem(int p, int act){                                       

  switch(act)

  {

    case 0:   value_cur[p] = pgm_read_word_near(actionInit + p + (SERVO_NUM * count_input));   break;

    case 1:   value_cur[p] = pgm_read_word_near(actionMove + p + (SERVO_NUM * count_input));   break;

    case 2:   value_cur[p] = pgm_read_word_near(actionBack + p + (SERVO_NUM * count_input));   break;

    case 3:   value_cur[p] = pgm_read_word_near(actionLeft + p + (SERVO_NUM * count_input));   break;

    case 4:   value_cur[p] = pgm_read_word_near(actionRight + p + (SERVO_NUM * count_input));   break;

    default: break;

  }

}

void servo_init(int act, int num){                         

  if(!_b)

  {

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

    {

      readProgmem(i, act);

      ServoGo(i, value_cur[i]);

      value_pre[i] = value_cur[i];

    }

  }

  num == 1 ? _b = true : _b = false;

}

void servo_move(int act, int num){           

  float value_delta[SERVO_NUM] = {};

  float in_value[SERVO_NUM] = {};

  servo_init(act, num);

  for(int i=0;i< num * actPwmNum[act];i++)

  {

    count_input++;

    if(count_input == actPwmNum[act])

    {

      count_input = 0;

      continue;

    }

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

    {

      readProgmem(i, act);

      in_value[i] = value_pre[i];

      value_delta[i] = (value_cur[i] - value_pre[i]) / frequency;

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

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

//      Serial.print(" ");

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

//      Serial.print(" ");

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

//      Serial.println();

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

    }

//    Serial.println();

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

    {

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

      {

        in_value[k] += value_delta[k];  

        value_pre[k] = in_value[k];

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

//        Serial.print(in_value[k]);

//        Serial.print(" ");

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

      }

//      Serial.println();

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

      {       

        ServoGo(j, in_value[j]);

      }

      delayMicroseconds(SERVO_SPEED);

    }

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

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

//    {

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

//    }

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

  }

}