SHARE
TWEET

Untitled

a guest Sep 19th, 2019 92 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /**********************************************************************
  2.  * pca9685_servo_easing_function_test.ino
  3.  * PCA9685 - 伺服馬達的緩動函數控制範例
  4.  *
  5.  * Author: Ruten.Proteus
  6.  * Date: 2019/09/06
  7.  *
  8.  * Written by Ruten.Proteus
  9.  *  BSD license, all text above must be included in any redistribution
  10.  * ********************************************************************
  11.  *
  12.  * {Hardware}
  13.  *    1. NodeMCU (ESP8266) + PCA9685 Servo Driver Board
  14.  *    2. Arduino Nano (ATmega328) + PCA9685 Servo Driver Board
  15.  *
  16.  * {Wiring}
  17.  *    NodeMCU     PCA9685  |   Arduino UNO/Nano     Power Supply
  18.  *  --------------------------------------------------------------
  19.  *     Vin                 |                           +5V  
  20.  *                  V+     |                           +5V
  21.  *                  VCC    |                           +5V
  22.  *                         |        5V                 +5V
  23.  *     GND          GND    |        GND                GND
  24.  *     D1(5)        SCL    |        A5
  25.  *     D2(4)        SDA    |        A4
  26.  *  --------------------------------------------------------------
  27.  *    SERVO      PCA9685
  28.  *  ----------------------
  29.  *     棕          GND
  30.  *     紅          V+
  31.  *     橙          PWM
  32.  * -----------------------
  33.  *
  34.  * {Result} Work !!!
  35.  *    + 程式功能實作新增的部分,?: 待測試;o: 已過測試;-: 功能移除;x: 未過測試
  36.  *    [o] Arduino Nano test OK!
  37.  *    [o] ESP8266, NodeMCU test OK!
  38.  *    [o] 可處理執行時間小於 SLICE_TIME 的需求,讓其全速旋轉至指定的角度。
  39.  *    [o] 可隨意指定或加入 Easing Function 作為轉動時的動作依據。
  40.  *
  41.  */
  42.  
  43. #include <Adafruit_PWMServoDriver.h>
  44.  
  45. #define MIN       0
  46. #define MAX       1
  47.  
  48. Adafruit_PWMServoDriver SVDRIVER;
  49.  
  50. /** easingEQ 定義緩動方程式 */
  51. typedef float (*EasingEQ)( float tick, float start_pos, float next_pos, float tick_count );
  52.  
  53. // cubic easing in/out - acceleration until halfway, then deceleration
  54. float easeInOutCubic(float t, float b, float c, float d) {
  55.    t /= d/2;
  56.    if (t < 1) return c/2*t*t*t + b;
  57.    t -= 2;
  58.    return c/2*(t*t*t + 2) + b;
  59. };
  60.  
  61. // sinusoidal easing in/out - accelerating until halfway, then decelerating
  62. float easeInOutSine(float t, float b, float c, float d) {
  63.    return -c/2 * (cos(PI*t/d) - 1) + b;
  64. };
  65.  
  66. //simple linear tweening - no easing, no acceleration
  67. float linearTween(float t, float b, float c, float d) {
  68.    return c*t/d + b;
  69. };
  70.  
  71. const uint8_t SERVOMOVENO = 4;
  72. const float SERVOMOVE[][2] = {
  73. //   deg,   ms }
  74.    {  90,  500 },
  75.    { 180,  500 },
  76.    {   0, 1000 },
  77.    { 180, 1000 },
  78. };
  79.  
  80. const uint8_t EASINGEQNO = 3;
  81. EasingEQ EASINGEQ[] = {
  82.    linearTween,
  83.    easeInOutSine,
  84.    easeInOutCubic,
  85. };
  86.  
  87. char* EASINGEQNAME[] = {
  88.    "linearTween",
  89.    "easeInOutSine",
  90.    "easeInOutCubic",
  91. };
  92.  
  93. // 伺服馬達脈衝寬度與角度設定
  94. int SERVO_PULSE_WIDTH_US[]       = { 700, 2400 };     // min ~ max = 700 ~ 2400 us
  95. int SERVO_ROTATE_ANGLE_DEGREE[]  = {   0,  180 };     // 對應上面的為 min ~ max = 0 ~ 180 度
  96.  
  97. // 儲存伺服馬達使用的 PCA9685 腳位
  98. const int SERVOS_PIN[8] = { 11, 10, 9, 8, 7, 6, 5, 4 };
  99. // 儲存伺服馬達位置
  100. int CURRENT_SERVOS_POSITION[8] = {90};
  101. // 切分最小時間(ms)
  102. const int SLICE_TIME = 10;    // 10 ms
  103.  
  104. unsigned long PREVIOUS_MILLIS;
  105. // EASINGEQ 計算需要的參數
  106. int _tick, _tick_count, _start_angle, _included_angle;
  107. // index for SERVOMOVE
  108. uint8_t _idx_servomove, _idx_easingeq;
  109.  
  110. // 回傳給 PCA9685 的 pwm 數值
  111. int degree_to_pwm_count( float degree ) {
  112.    return int( map( degree, SERVO_ROTATE_ANGLE_DEGREE[MIN], SERVO_ROTATE_ANGLE_DEGREE[MAX],
  113.                             SERVO_PULSE_WIDTH_US[MIN], SERVO_PULSE_WIDTH_US[MAX]) * 0.2048 );
  114. }
  115.  
  116. void setup() {
  117.    Serial.begin( 115200 );
  118.    Serial.print("\r\n\r\n");
  119.  
  120.    SVDRIVER.begin();
  121.    SVDRIVER.setPWMFreq( 50 );   // 50Hz = 20ms = 4096 (修改此處會影響 defree_to_pwm_count)
  122.    Wire.setClock(400000);
  123.  
  124.    delay( 2000 );
  125.  
  126.    // 回到預設點
  127.    Serial.println( F("Move to initial position: 90") );
  128.    SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(90) );
  129.    delay( 1000 );
  130.  
  131.    Serial.println( EASINGEQNAME[0] );
  132.  
  133.    // 相關參數初始化
  134.    _tick = _tick_count = _idx_servomove = _included_angle = 0;
  135.    PREVIOUS_MILLIS = millis();
  136. }
  137.  
  138. void loop() {
  139.  
  140.    if( (millis() - PREVIOUS_MILLIS) >= SLICE_TIME ) {
  141.  
  142.       PREVIOUS_MILLIS = millis();
  143.  
  144.       if( _tick == 0 ) {
  145.          // 計算切分的數量
  146.          if( SERVOMOVE[_idx_servomove][1] < SLICE_TIME ) { _tick = _tick_count = 1; }
  147.          else _tick_count = SERVOMOVE[_idx_servomove][1] / SLICE_TIME;
  148.          // 夾角
  149.          _included_angle = SERVOMOVE[_idx_servomove][0] - CURRENT_SERVOS_POSITION[0];
  150.          // 起始角度
  151.          _start_angle = CURRENT_SERVOS_POSITION[0];
  152.          Serial.print( "\tmove to "); Serial.print( SERVOMOVE[_idx_servomove][0] );
  153.          Serial.print( "deg, " ); Serial.print( SERVOMOVE[_idx_servomove][1] ); Serial.println( " ms" );
  154.          if( ++_idx_servomove == SERVOMOVENO ) {                     // 一個 SERVOMLOVE 指定的循環角度執行完畢
  155.             _idx_servomove = 0;
  156.             if( ++_idx_easingeq == EASINGEQNO ) _idx_easingeq = 0;   // 切換 EASINGEQ
  157.             Serial.println( EASINGEQNAME[_idx_easingeq] );
  158.          }
  159.       }
  160.  
  161.       // 切分執行
  162.       CURRENT_SERVOS_POSITION[0] = constrain( EASINGEQ[_idx_easingeq]( _tick, _start_angle, _included_angle, _tick_count ), 0, 180 );
  163.       SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(CURRENT_SERVOS_POSITION[0]) );
  164.  
  165.       // 角度到達後
  166.       if( ++_tick >= _tick_count ) _tick = 0;
  167.    }  
  168. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top