/**********************************************************************
 * pca9685_servo_easing_function_test.ino
 * PCA9685 - 伺服馬達的緩動函數控制範例
 *
 * Author: Ruten.Proteus
 * Date: 2019/09/06
 *
 * Written by Ruten.Proteus
 *  BSD license, all text above must be included in any redistribution
 * ********************************************************************
 *
 * {Hardware}
 *    1. NodeMCU (ESP8266) + PCA9685 Servo Driver Board
 *    2. Arduino Nano (ATmega328) + PCA9685 Servo Driver Board
 *
 * {Wiring}
 *    NodeMCU     PCA9685  |   Arduino UNO/Nano     Power Supply
 *  --------------------------------------------------------------
 *     Vin                 |                           +5V
 *                  V+     |                           +5V
 *                  VCC    |                           +5V
 *                         |        5V                 +5V
 *     GND          GND    |        GND                GND
 *     D1(5)        SCL    |        A5
 *     D2(4)        SDA    |        A4
 *  --------------------------------------------------------------
 *    SERVO      PCA9685
 *  ----------------------
 *     棕          GND
 *     紅          V+
 *     橙          PWM
 * -----------------------
 *
 * {Result} Work !!!
 *    + 程式功能實作新增的部分，?: 待測試；o: 已過測試；-: 功能移除；x: 未過測試
 *    [o] Arduino Nano test OK!
 *    [o] ESP8266, NodeMCU test OK!
 *    [o] 可處理執行時間小於 SLICE_TIME 的需求，讓其全速旋轉至指定的角度。
 *    [o] 可隨意指定或加入 Easing Function 作為轉動時的動作依據。
 *
 */

#include <Adafruit_PWMServoDriver.h>

#define MIN       0
#define MAX       1

Adafruit_PWMServoDriver SVDRIVER;

/** easingEQ 定義緩動方程式 */
typedef float (*EasingEQ)( float tick, float start_pos, float next_pos, float tick_count );

// cubic easing in/out - acceleration until halfway, then deceleration
float easeInOutCubic(float t, float b, float c, float d) {
   t /= d/2;
   if (t < 1) return c/2*t*t*t + b;
   t -= 2;
   return c/2*(t*t*t + 2) + b;
};

// sinusoidal easing in/out - accelerating until halfway, then decelerating
float easeInOutSine(float t, float b, float c, float d) {
   return -c/2 * (cos(PI*t/d) - 1) + b;
};

//simple linear tweening - no easing, no acceleration
float linearTween(float t, float b, float c, float d) {
   return c*t/d + b;
};

const uint8_t SERVOMOVENO = 4;
const float SERVOMOVE[][2] = {
//   deg,   ms }
   {  90,  500 },
   { 180,  500 },
   {   0, 1000 },
   { 180, 1000 },
};

const uint8_t EASINGEQNO = 3;
EasingEQ EASINGEQ[] = {
   linearTween,
   easeInOutSine,
   easeInOutCubic,
};

char* EASINGEQNAME[] = {
   "linearTween",
   "easeInOutSine",
   "easeInOutCubic",
};

// 伺服馬達脈衝寬度與角度設定
int SERVO_PULSE_WIDTH_US[]       = { 700, 2400 };     // min ~ max = 700 ~ 2400 us
int SERVO_ROTATE_ANGLE_DEGREE[]  = {   0,  180 };     // 對應上面的為 min ~ max = 0 ~ 180 度

// 儲存伺服馬達使用的 PCA9685 腳位
const int SERVOS_PIN[8] = { 11, 10, 9, 8, 7, 6, 5, 4 };
// 儲存伺服馬達位置
int CURRENT_SERVOS_POSITION[8] = {90};
// 切分最小時間（ms）
const int SLICE_TIME = 10;    // 10 ms

unsigned long PREVIOUS_MILLIS;
// EASINGEQ 計算需要的參數
int _tick, _tick_count, _start_angle, _included_angle;
// index for SERVOMOVE
uint8_t _idx_servomove, _idx_easingeq;

// 回傳給 PCA9685 的 pwm 數值
int degree_to_pwm_count( float degree ) {
   return int( map( degree, SERVO_ROTATE_ANGLE_DEGREE[MIN], SERVO_ROTATE_ANGLE_DEGREE[MAX],
                            SERVO_PULSE_WIDTH_US[MIN], SERVO_PULSE_WIDTH_US[MAX]) * 0.2048 );
}

void setup() {
   Serial.begin( 115200 );
   Serial.print("\r\n\r\n");

   SVDRIVER.begin();
   SVDRIVER.setPWMFreq( 50 );   // 50Hz = 20ms = 4096 (修改此處會影響 defree_to_pwm_count)
   Wire.setClock(400000);

   delay( 2000 );

   // 回到預設點
   Serial.println( F("Move to initial position: 90") );
   SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(90) );
   delay( 1000 );

   Serial.println( EASINGEQNAME[0] );

   // 相關參數初始化
   _tick = _tick_count = _idx_servomove = _included_angle = 0;
   PREVIOUS_MILLIS = millis();
}

void loop() {

   if( (millis() - PREVIOUS_MILLIS) >= SLICE_TIME ) {

      PREVIOUS_MILLIS = millis();

      if( _tick == 0 ) {
         // 計算切分的數量
         if( SERVOMOVE[_idx_servomove][1] < SLICE_TIME ) { _tick = _tick_count = 1; }
         else _tick_count = SERVOMOVE[_idx_servomove][1] / SLICE_TIME;
         // 夾角
         _included_angle = SERVOMOVE[_idx_servomove][0] - CURRENT_SERVOS_POSITION[0];
         // 起始角度
         _start_angle = CURRENT_SERVOS_POSITION[0];
         Serial.print( "\tmove to "); Serial.print( SERVOMOVE[_idx_servomove][0] );
         Serial.print( "deg, " ); Serial.print( SERVOMOVE[_idx_servomove][1] ); Serial.println( " ms" );
         if( ++_idx_servomove == SERVOMOVENO ) {                     // 一個 SERVOMLOVE 指定的循環角度執行完畢
            _idx_servomove = 0;
            if( ++_idx_easingeq == EASINGEQNO ) _idx_easingeq = 0;   // 切換 EASINGEQ
            Serial.println( EASINGEQNAME[_idx_easingeq] );
         }
      }

      // 切分執行
      CURRENT_SERVOS_POSITION[0] = constrain( EASINGEQ[_idx_easingeq]( _tick, _start_angle, _included_angle, _tick_count ), 0, 180 );
      SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(CURRENT_SERVOS_POSITION[0]) );

      // 角度到達後
      if( ++_tick >= _tick_count ) _tick = 0;
   }
}