├── .gitignore ├── CONTRIBUTORS.md ├── keywords.txt ├── library.properties ├── examples ├── Read │ └── Read.ino ├── MultiMotors │ └── MultiMotors.ino └── Rotation │ └── Rotation.ino ├── SUPPORTED.md ├── LICENSE ├── src ├── FeedBackServo.h └── FeedBackServo.cpp ├── sketch_sample.ino ├── README_ja.md └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /CONTRIBUTORS.md: -------------------------------------------------------------------------------- 1 | # Parallax FeedBack 360 Servo Control Library 4 Arduino Contributors 2 | 3 | We sincerely thank everyone who has contributed to this project. 4 | 5 | Their contributions are essential to the project's development. 6 | 7 | --- 8 | 9 | ## Contributors 10 | 11 | - **[ibrahimseleem](https://github.com/ibrahimseleem)** 12 | - **[Yupopyoi](https://github.com/Yupopyoi)** 13 | - **[dmfgd1](https://github.com/dmfgd1)** 14 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | FeedBackServo KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | setServoControl KEYWORD2 14 | rotate KEYWORD2 15 | Angle KEYWORD2 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name= Parallax FeedBack360 Servo Control Library 2 | version=2.0.1 3 | author=Hyoda Kazuaki 4 | maintainer=Hyoda Kazuaki 5 | sentence=A library that makes control Parallax FeedBack360 servo easy. 6 | paragraph=You can control multiple servo motors to degree of rotation what you want with this library. Of course, you can read servo's degree of rotation. 7 | category=Device Control 8 | url=https://github.com/HyodaKazuaki/FeedBack360-Servo-Control-Library-4-Arduino 9 | architectures=avr,sam 10 | -------------------------------------------------------------------------------- /examples/Read/Read.ino: -------------------------------------------------------------------------------- 1 | #include "FeedBackServo.h" 2 | 3 | // Use a valid interrupt pin 4 | #define FEEDBACK_PIN 2 5 | 6 | // Create the servo object 7 | FeedBackServo feedbackServo(FEEDBACK_PIN); 8 | 9 | void setup() 10 | { 11 | Serial.begin(115200); 12 | } 13 | 14 | void loop() 15 | { 16 | // Necessary to ensure the measured angle of the servo is updated each iteration 17 | feedbackServo.update(); 18 | 19 | // Retrieve angle from servo 20 | int currentAngle = feedbackServo.getAngle(); 21 | 22 | // Output angle 23 | Serial.println(currentAngle); 24 | } 25 | -------------------------------------------------------------------------------- /examples/MultiMotors/MultiMotors.ino: -------------------------------------------------------------------------------- 1 | #include "FeedBackServo.h" 2 | // Define feedback signal pin 3 | #define FEEDBACK_PIN1 2 4 | #define FEEDBACK_PIN2 3 5 | // Define servo control pin number 6 | #define SERVO_PIN1 9 7 | #define SERVO_PIN2 10 8 | 9 | // Set feedback signal pin number 10 | FeedBackServo servo1 = FeedBackServo(FEEDBACK_PIN1); 11 | FeedBackServo servo2 = FeedBackServo(FEEDBACK_PIN2); 12 | 13 | void setup() 14 | { 15 | Serial.begin(115200); 16 | 17 | servo1.setServoControl(SERVO_PIN1); 18 | servo2.setServoControl(SERVO_PIN2); 19 | 20 | servo1.setTarget(300); 21 | servo2.setTarget(300); 22 | 23 | servo1.setKp(0.5); 24 | servo2.setKp(0.5); 25 | } 26 | 27 | void loop() 28 | { 29 | servo1.update(); 30 | servo2.update(); 31 | 32 | Serial.print(servo1.getAngle()); 33 | Serial.print(" / "); 34 | Serial.println(servo2.getAngle()); 35 | } 36 | -------------------------------------------------------------------------------- /SUPPORTED.md: -------------------------------------------------------------------------------- 1 | # Supported List 2 | 3 | If you have successfully used this library with an unconfirmed board, please let us know with a Pull Request. 4 | 5 | ◯ ... Supported 6 | △ ... Unconfirmed 7 | ╳ ... Not Supported 8 | 9 | | Board | Status | 10 | | --- | --- | 11 | | Arduino Yún | △ | 12 | | Arduino/Genuino Uno | ◯ | 13 | | Arduino Duemilanove or Diecimila | △ | 14 | | Arduino Nano | △ | 15 | | Arduino Nano IoT 33 | ◯ | 16 | | Arduino/Genuino Mega or Mega 2560 | ◯ | 17 | | Arduino Mega ADK | △ | 18 | | Arduino Leonardo | △ | 19 | | Arduino Leonardo ETH | △ | 20 | | Arduino/Genuino Micro | △ | 21 | | Arduino Esplora | △ | 22 | | Arduino Mini | △ | 23 | | Arduino Ethernet | △ | 24 | | Arduino Fio | △ | 25 | | Arduino BT | △ | 26 | | LilyPad Arduino USB | △ | 27 | | LilyPad Arduino | △ | 28 | | Arduino Pro or Pro Mini | △ | 29 | | Arduino NG or older | △ | 30 | | Arduino Robot Control | △ | 31 | | Arduino Robot Motor | △ | 32 | | Arduino Gemma | △ | 33 | | Arduino Circuit Playground | △ | 34 | | Arduino Yún Mini | △ | 35 | | Arduino Industrial 101 | △ | 36 | | Linino One | △ | 37 | | Arduino Uno WiFi | △ | 38 | | Arduino Due | △ | 39 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019-2025 Hyoda Kazuaki 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /examples/Rotation/Rotation.ino: -------------------------------------------------------------------------------- 1 | #include "FeedBackServo.h" 2 | 3 | // Sefine feedback signal pin and servo control pin 4 | #define FEEDBACK_PIN 2 5 | #define SERVO_PIN 3 6 | 7 | // Set feedback signal pin number 8 | FeedBackServo servo = FeedBackServo(FEEDBACK_PIN); 9 | 10 | int target = 0; // State selection 11 | const long interval = 2000; // 2 seconds (in milliseconds) 12 | unsigned long previousTime = 0; 13 | 14 | void setup() 15 | { 16 | // Set servo control pin number 17 | servo.setServoControl(SERVO_PIN); 18 | 19 | // Adjust Kp as needed 20 | servo.setKp(1.0); 21 | } 22 | 23 | void loop() 24 | { 25 | // Rotate servo from 0 to 180 (w/ +-2 threshold) using non-blocking. 26 | servo.update(2); 27 | 28 | // Calculate whether new target input request meets specified time interval requirement to prevent mistarget 29 | unsigned long currentTime = millis(); 30 | if (currentTime - previousTime >= interval) 31 | { 32 | previousTime = currentTime; 33 | 34 | // Prevents improper targetting by providing proper time for relevant calculations to take place 35 | switch (target) 36 | { 37 | case 0: 38 | target = 1; 39 | servo.setTarget(0); 40 | break; 41 | case 1: 42 | target = 0; 43 | servo.setTarget(180); 44 | break; 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/FeedBackServo.h: -------------------------------------------------------------------------------- 1 | #ifndef FEEDBACK360_CONTROL_LIBRARY 2 | #define FEEDBACK360_CONTROL_LIBRARY 3 | 4 | #include 5 | #include 6 | 7 | #ifdef ARDUINO_AVR_UNO 8 | #define MAX_INTERRUPT_NUM 2 9 | #endif 10 | 11 | #ifdef ARDUINO_AVR_LEONARDO 12 | #define MAX_INTERRUPT_NUM 5 13 | #endif 14 | 15 | #ifdef ARDUINO_AVR_MEGA2560 16 | #define MAX_INTERRUPT_NUM 6 17 | #endif 18 | 19 | #ifdef ARDUINO_SAMD_NANO_33_IOT 20 | #define MAX_INTERRUPT_NUM 9 21 | #endif 22 | 23 | // If not listed above, leave it at 2. 24 | // This is just a provisional value; some boards may require a larger value. 25 | #ifndef MAX_INTERRUPT_NUM 26 | #define MAX_INTERRUPT_NUM 2 27 | #endif 28 | 29 | class FeedBackServo 30 | { 31 | public: 32 | FeedBackServo(byte feedbackPinNumber); 33 | void setServoControl(byte servoPinNumber); 34 | void setKp(float Kp = 1.0); 35 | void setActive(bool isActive); 36 | void setTarget(int target); 37 | void update(int threshold = 4); 38 | int getAngle(); 39 | 40 | // These functions are left for compatibility. 41 | void rotate(int degree, int threshold = 4); 42 | int Angle(); 43 | 44 | private: 45 | void checkPin(byte pinNumber); 46 | void handleFeedback(); 47 | void updateAngleFromPWM(); 48 | 49 | Servo parallax_; 50 | byte feedbackPinNumber_; 51 | byte interruptNumber_; 52 | 53 | float Kp_ = 1.0; 54 | bool isActive_ = true; 55 | int targetAngle_; 56 | 57 | volatile int angle_ = 0; 58 | float thetaPre_ = 0; 59 | unsigned int tHigh_ = 0, tLow_ = 0; 60 | unsigned long rise_ = 0, fall_ = 0; 61 | int turns_ = 0; 62 | 63 | volatile bool feedbackUpdated_ = false; 64 | 65 | static const int UNITS_FC = 360; // Full Circle 66 | static const float DC_MIN; 67 | static const float DC_MAX; 68 | static const int DUTY_SCALE = 1; 69 | static const int Q2_MIN; 70 | static const int Q3_MAX; 71 | 72 | static FeedBackServo* instances[MAX_INTERRUPT_NUM]; 73 | static void isr0(); 74 | static void isr1(); 75 | static void isr2(); 76 | static void isr3(); 77 | static void isr4(); 78 | static void isr5(); 79 | }; 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /sketch_sample.ino: -------------------------------------------------------------------------------- 1 | // このファイルはArduino Unoで動作を確認するために使用したスケッチです。 2 | // targetAngleに指定した角度にモーターが回転します。 3 | 4 | #include 5 | 6 | Servo Parallax; 7 | 8 | // set signal pin to Pin no.2 9 | volatile float angle, targetAngle; 10 | int theta = 0, thetaP = 0; 11 | volatile unsigned long tHigh = 0, tLow = 0; 12 | volatile unsigned long rise = 0, fall = 0; 13 | volatile float dc; 14 | volatile int tCycle; 15 | int turns = 0; 16 | int Kp = 1; 17 | int unitsFC = 360; 18 | float dcMin = 0.029; 19 | float dcMax = 0.971; 20 | float dutyScale = 1.0; 21 | float q2min = unitsFC / 4.0; 22 | float q3max = q2min * 3.0; 23 | 24 | void setup() { 25 | // put your setup code here, to run once: 26 | Parallax.attach(3); 27 | attachInterrupt(0, feedback360, CHANGE); 28 | Serial.begin(115200); 29 | } 30 | 31 | void loop() { 32 | // put your main code here, to run repeatedly: 33 | int errorAngle, output, offset, value; 34 | targetAngle = 270; 35 | 36 | Serial.print(" Angle: "); 37 | Serial.println(angle); 38 | 39 | errorAngle = targetAngle - angle; 40 | output = errorAngle * Kp; 41 | if(output > 200) 42 | output = 200; 43 | if(output < -200) 44 | output = -200; 45 | if(errorAngle > 0) 46 | offset = 30; 47 | else if(errorAngle < 0) 48 | offset = -30; 49 | else 50 | offset = 0; 51 | value = (abs(errorAngle) < 4) ? 0 : output + offset; 52 | Parallax.writeMicroseconds(1490 - value); 53 | } 54 | 55 | void feedback360() { 56 | if(digitalRead(2)) { 57 | // rise 58 | rise = micros(); 59 | tLow = rise - fall; 60 | 61 | // Calcuate duty 62 | tCycle = tHigh + tLow; 63 | if((tCycle < 1000) && (tCycle > 1200)) 64 | return; 65 | 66 | dc = (dutyScale * tHigh) / (float)tCycle; 67 | theta = ((dc - dcMin) * unitsFC) / (dcMax - dcMin); 68 | 69 | if(theta < 0) 70 | theta = 0; 71 | else if(theta > (unitsFC - 1)) 72 | theta = unitsFC - 1; 73 | 74 | if((theta < q2min) && (thetaP > q3max)) 75 | turns++; 76 | else if((thetaP < q2min) && (theta > q3max)) 77 | turns--; 78 | 79 | if(turns >= 0) 80 | angle = (turns * unitsFC) + theta; 81 | else if(turns < 0) 82 | angle = ((turns + 1) * unitsFC) - (unitsFC - theta); 83 | 84 | thetaP = theta; 85 | 86 | } else { 87 | // fall 88 | fall = micros(); 89 | tHigh = fall - rise; 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /README_ja.md: -------------------------------------------------------------------------------- 1 | # Parallax FeedBack 360 Servo Control Library 4 Arduino 2 | 3 | このライブラリは[Parallax FeedBack 360° High Speed Servo](https://www.parallax.com/product/900-00360)の制御を容易にします。このサーボモーターは PWM 信号によるフィードバックが行われます。このライブラリを使用して、サーボの回転角度を制御できる他、サーボの回転角度を読み取ることができます。 4 | 5 | English README is [`README.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/README.md). 6 | 7 | ## お知らせ 8 | 9 | このライブラリは、メジャーバージョン 2.0.0 以降、非同期制御と複数のサーボモーター制御のため破壊的変更を導入しました。 10 | 詳細は関数定義やサンプルをご確認ください。 11 | 12 | ## サポート 13 | 14 | [`SUPPORTED.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/SUPPORTED.md)をご覧ください。 15 | 16 | ## インストール方法 17 | 18 | [Release](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/releases)より zip ファイルをダウンロードし、Arduino IDE のメニューにある`スケッチ`→`ライブラリをインクルード`→`.zip形式のライブラリをインストール`を選択してダウンロードした zip ファイルをインストールします。 19 | 20 | ## ピン番号 21 | 22 | フィードバック信号ピンは外部割り込みができるピンを指定し、そこにサーボモーターのフィードバック信号線を割り当ててください。 23 | 制御ピンにはサーボモーターの制御線を割り当てたピンを指定してください。 24 | 25 | ## サンプル 26 | 27 | このライブラリにはいくつかのサンプルが含まれており、`ファイル`→`スケッチ例`より確認することができます。 28 | 29 | ### Rotation 30 | 31 | ```cpp 32 | #include "FeedBackServo.h" 33 | 34 | // フィードバック信号ピンとサーボモータ制御ピンを定義 35 | #define FEEDBACK_PIN 2 36 | #define SERVO_PIN 3 37 | 38 | // フィードバック信号ピンをセット 39 | FeedBackServo servo = FeedBackServo(FEEDBACK_PIN); 40 | 41 | int target = 0; // ターゲット状態変数 42 | const long interval = 2000; // 2秒(2000ミリ秒) 43 | unsigned long previousTime = 0; 44 | 45 | void setup() 46 | { 47 | // サーボモータ制御ピンをセット 48 | servo.setServoControl(SERVO_PIN); 49 | 50 | // P制御用のKp値をセット 51 | servo.setKp(1.0); 52 | } 53 | 54 | void loop() 55 | { 56 | // サーボモータをノンブロッキングに2秒ごとに0度、180度に回転(+-2度の誤差を含む) 57 | servo.update(2); 58 | 59 | // 前回時間から2秒経過するまでスキップし処理時間を確保 60 | unsigned long currentTime = millis(); 61 | if (currentTime - previousTime >= interval) 62 | { 63 | previousTime = currentTime; 64 | 65 | // ターゲット状態変数に応じて確度を変更 66 | switch (target) 67 | { 68 | case 0: 69 | target = 1; 70 | servo.setTarget(0); 71 | break; 72 | case 1: 73 | target = 0; 74 | servo.setTarget(180); 75 | break; 76 | } 77 | } 78 | } 79 | ``` 80 | 81 | ### Read 82 | 83 | ```cpp 84 | #include "FeedBackServo.h" 85 | 86 | // フィードバック信号ピンを定義 87 | #define FEEDBACK_PIN 2 88 | 89 | // フィードバック信号ピンをセット 90 | FeedBackServo feedbackServo(FEEDBACK_PIN); 91 | 92 | void setup() { 93 | // シリアル通信を115200bpsで開始 94 | Serial.begin(115200); 95 | } 96 | 97 | void loop() { 98 | // サーボの角度を計測するためupdateメンバー関数の呼び出しが必要 99 | feedbackServo.update(); 100 | 101 | // サーボの角度を取得 102 | int currentAngle = feedbackServo.getAngle(); 103 | 104 | Serial.println(currentAngle); 105 | } 106 | ``` 107 | 108 | ### Multi motor controls 109 | 110 | ```cpp 111 | #include "FeedBackServo.h" 112 | // フィードバック信号ピンを定義 113 | #define FEEDBACK_PIN1 2 114 | #define FEEDBACK_PIN2 3 115 | // サーボモータ制御ピンを定義 116 | #define SERVO_PIN1 9 117 | #define SERVO_PIN2 10 118 | 119 | // フィードバック信号ピンをセット 120 | FeedBackServo servo1 = FeedBackServo(FEEDBACK_PIN1); 121 | FeedBackServo servo2 = FeedBackServo(FEEDBACK_PIN2); 122 | 123 | void setup() 124 | { 125 | Serial.begin(115200); 126 | 127 | servo1.setServoControl(SERVO_PIN1); 128 | servo2.setServoControl(SERVO_PIN2); 129 | 130 | servo1.setTarget(300); 131 | servo2.setTarget(300); 132 | 133 | servo1.setKp(0.5); 134 | servo2.setKp(0.5); 135 | } 136 | 137 | void loop() 138 | { 139 | servo1.update(); 140 | servo2.update(); 141 | 142 | Serial.print(servo1.getAngle()); 143 | Serial.print(" / "); 144 | Serial.println(servo2.getAngle()); 145 | } 146 | ``` 147 | 148 | ## ライセンス 149 | 150 | このライブラリは MIT ライセンスに基づいて配布させています。 151 | 詳しくは[`LICENSE`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/LICENSE)をご覧ください。 152 | 153 | ## 貢献者 154 | 155 | このライブラリは多くの個人の貴重な貢献によって支えられています。 156 | 詳細な貢献者リストは[`CONTRIBUTORS.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/CONTRIBUTORS.md)をご覧ください。 157 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Parallax FeedBack 360 Servo Control Library 4 Arduino 2 | 3 | This library facilitates control of [Parallax FeedBack 360° High Speed Servo](https://www.parallax.com/product/900-00360). This servo motor provides feedback via the PWM signal. You can control servo to degree of rotation what you want with this library. Of course, you can read servo's degree of rotation. 4 | 5 | 日本語の README は[`README_ja.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/README_ja.md)をご覧ください。 6 | 7 | ## NOTICE 8 | 9 | This library introduced breaking changes for non-blocking control and support for multiple servo motors since major version 2.0.0. 10 | For details, see function definitions and examples. 11 | 12 | ## Support 13 | 14 | Please see [`SUPPORTED.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/SUPPORTED.md). 15 | 16 | ## Pin Assign 17 | 18 | Assign the feedback signal line of the servomotor to the feedback signal pin that allows external interruption. 19 | Assign the control pin to which the servo motor control line is assigned. 20 | 21 | ## How to Install 22 | 23 | Download this library on [Release page](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/releases) and Install zip file on Arduino IDE. 24 | 25 | ## Example 26 | 27 | This library is contains some examples. 28 | 29 | ### Rotation 30 | 31 | ```cpp 32 | #include "FeedBackServo.h" 33 | 34 | // Sefine feedback signal pin and servo control pin 35 | #define FEEDBACK_PIN 2 36 | #define SERVO_PIN 3 37 | 38 | // Set feedback signal pin number 39 | FeedBackServo servo = FeedBackServo(FEEDBACK_PIN); 40 | 41 | int target = 0; // State selection 42 | const long interval = 2000; // 2 seconds (in milliseconds) 43 | unsigned long previousTime = 0; 44 | 45 | void setup() 46 | { 47 | // Set servo control pin number 48 | servo.setServoControl(SERVO_PIN); 49 | 50 | // Adjust Kp as needed 51 | servo.setKp(1.0); 52 | } 53 | 54 | void loop() 55 | { 56 | // Rotate servo from 0 to 180 (w/ +-2 threshold) using non-blocking. 57 | servo.update(2); 58 | 59 | // Calculate whether new target input request meets specified time interval requirement to prevent mistarget 60 | unsigned long currentTime = millis(); 61 | if (currentTime - previousTime >= interval) 62 | { 63 | previousTime = currentTime; 64 | 65 | // Prevents improper targetting by providing proper time for relevant calculations to take place 66 | switch (target) 67 | { 68 | case 0: 69 | target = 1; 70 | servo.setTarget(0); 71 | break; 72 | case 1: 73 | target = 0; 74 | servo.setTarget(180); 75 | break; 76 | } 77 | } 78 | } 79 | ``` 80 | 81 | ### Read 82 | 83 | ```cpp 84 | #include "FeedBackServo.h" 85 | 86 | // Use a valid interrupt pin 87 | #define FEEDBACK_PIN 2 88 | 89 | // Create the servo object 90 | FeedBackServo feedbackServo(FEEDBACK_PIN); 91 | 92 | void setup() 93 | { 94 | Serial.begin(115200); 95 | } 96 | 97 | void loop() 98 | { 99 | // Necessary to ensure the measured angle of the servo is updated each iteration 100 | feedbackServo.update(); 101 | 102 | // Retrieve angle from servo 103 | int currentAngle = feedbackServo.getAngle(); 104 | 105 | // Output angle 106 | Serial.println(currentAngle); 107 | } 108 | ``` 109 | 110 | ### Multi motor controls 111 | 112 | ```cpp 113 | #include "FeedBackServo.h" 114 | // Define feedback signal pin 115 | #define FEEDBACK_PIN1 2 116 | #define FEEDBACK_PIN2 3 117 | // Define servo control pin number 118 | #define SERVO_PIN1 9 119 | #define SERVO_PIN2 10 120 | 121 | // Set feedback signal pin number 122 | FeedBackServo servo1 = FeedBackServo(FEEDBACK_PIN1); 123 | FeedBackServo servo2 = FeedBackServo(FEEDBACK_PIN2); 124 | 125 | void setup() 126 | { 127 | Serial.begin(115200); 128 | 129 | servo1.setServoControl(SERVO_PIN1); 130 | servo2.setServoControl(SERVO_PIN2); 131 | 132 | servo1.setTarget(300); 133 | servo2.setTarget(300); 134 | 135 | servo1.setKp(0.5); 136 | servo2.setKp(0.5); 137 | } 138 | 139 | void loop() 140 | { 141 | servo1.update(); 142 | servo2.update(); 143 | 144 | Serial.print(servo1.getAngle()); 145 | Serial.print(" / "); 146 | Serial.println(servo2.getAngle()); 147 | } 148 | ``` 149 | 150 | ## License 151 | 152 | This library is released under the MIT License. 153 | Please see [`LICENSE`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/LICENSE) file for detail. 154 | 155 | ## Contributors 156 | 157 | This library is made possible by the valuable contributions of many individuals. 158 | Please see [`CONTRIBUTORS.md`](https://github.com/HyodaKazuaki/Parallax-FeedBack-360-Servo-Control-Library-4-Arduino/blob/master/CONTRIBUTORS.md) file for a detailed list. 159 | -------------------------------------------------------------------------------- /src/FeedBackServo.cpp: -------------------------------------------------------------------------------- 1 | // FeedBack360 Servo Control Library 4 Arduino 2 | // Controll.cpp 3 | // Copyright © Hyoda Kazuaki 4 | // Parallax Feedback 360° High Speed Servo is made by Parallax Inc. 5 | // This library is released under the MIT License. 6 | 7 | #include "FeedBackServo.h" 8 | 9 | // Constants for duty cycle interpretation (based on Parallax spec) 10 | const float FeedBackServo::DC_MIN = 0.029; 11 | const float FeedBackServo::DC_MAX = 0.971; 12 | const int FeedBackServo::Q2_MIN = FeedBackServo::UNITS_FC / 4; 13 | const int FeedBackServo::Q3_MAX = FeedBackServo::Q2_MIN * 3; 14 | 15 | FeedBackServo* FeedBackServo::instances[MAX_INTERRUPT_NUM] = { nullptr }; 16 | 17 | FeedBackServo::FeedBackServo(byte feedbackPinNumber) 18 | { 19 | // feedback pin number validation 20 | checkPin(feedbackPinNumber); 21 | feedbackPinNumber_ = feedbackPinNumber; 22 | 23 | // convert feedback pin number to interrupt number for use on attachInterrupt function 24 | interruptNumber_ = digitalPinToInterrupt(feedbackPinNumber_); 25 | 26 | if (interruptNumber_ < MAX_INTERRUPT_NUM) { 27 | instances[interruptNumber_] = this; 28 | switch (interruptNumber_) 29 | { 30 | case 0: attachInterrupt(0, isr0, CHANGE); break; 31 | case 1: attachInterrupt(1, isr1, CHANGE); break; 32 | case 2: attachInterrupt(2, isr2, CHANGE); break; 33 | case 3: attachInterrupt(3, isr3, CHANGE); break; 34 | case 4: attachInterrupt(4, isr4, CHANGE); break; 35 | case 5: attachInterrupt(5, isr5, CHANGE); break; 36 | } 37 | } 38 | } 39 | 40 | void FeedBackServo::setServoControl(byte servoPinNumber) 41 | { 42 | // Servo control pin attach 43 | parallax_.attach(servoPinNumber); 44 | } 45 | 46 | void FeedBackServo::setKp(float Kp) 47 | { 48 | FeedBackServo::Kp_ = Kp; 49 | } 50 | 51 | void FeedBackServo::setActive(bool isActive) 52 | { 53 | isActive_ = isActive; 54 | } 55 | 56 | void FeedBackServo::setTarget(int target) 57 | { 58 | targetAngle_ = target; 59 | } 60 | 61 | void FeedBackServo::update(int threshold) 62 | { 63 | // Update angle based on the latest PWM feedback 64 | updateAngleFromPWM(); 65 | 66 | if (isActive_ == false) return; 67 | 68 | int errorAngle = targetAngle_ - angle_; 69 | if (abs(errorAngle) <= threshold) 70 | { 71 | parallax_.writeMicroseconds(1490); 72 | return; 73 | } 74 | 75 | // NOTE: Using simple P-control. 76 | // TODO: PID control may improve stability and response. 77 | float output = constrain(errorAngle * Kp_, -200.0, 200.0); 78 | float offset = (errorAngle > 0) ? 30.0 : -30.0; 79 | float value = output + offset; 80 | 81 | parallax_.writeMicroseconds(1490 - value); 82 | } 83 | 84 | int FeedBackServo::getAngle() 85 | { 86 | return angle_; 87 | } 88 | 89 | /* 90 | * Legacy blocking rotation function for compatibility. 91 | * 92 | * This function sets the target angle and actively blocks until the target is reached 93 | * within the specified threshold. 94 | * 95 | * Note: 96 | * - Suitable only when controlling a single motor. 97 | * - Not recommended in multi-motor systems. 98 | * - For non-blocking control, call `setTarget()` and use `update()` in the main loop. 99 | */ 100 | [[deprecated("Use non-blocking control functions, setTarget(int) and update().")]] 101 | void FeedBackServo::rotate(int degree, int threshold) 102 | { 103 | setTarget(degree); 104 | 105 | while (abs(degree - angle_) >= threshold) 106 | { 107 | update(threshold); 108 | } 109 | } 110 | 111 | int FeedBackServo::Angle() 112 | { 113 | return getAngle(); 114 | } 115 | 116 | void FeedBackServo::checkPin(byte feedbackPinNumber) 117 | { 118 | // Check pin number 119 | #ifdef ARDUINO_AVR_UNO 120 | if (feedbackPinNumber != 2 && 121 | feedbackPinNumber != 3) 122 | exit(1); 123 | #endif 124 | #ifdef ARDUINO_AVR_LEONARDO 125 | if (feedbackPinNumber != 0 && 126 | feedbackPinNumber != 1 && 127 | feedbackPinNumber != 2 && 128 | feedbackPinNumber != 3 && 129 | feedbackPinNumber != 7) 130 | exit(1); 131 | #endif 132 | #ifdef ARDUINO_AVR_MEGA2560 133 | if (feedbackPinNumber != 2 && 134 | feedbackPinNumber != 3 && 135 | feedbackPinNumber != 18 && 136 | feedbackPinNumber != 19 && 137 | feedbackPinNumber != 20 && 138 | feedbackPinNumber != 21) 139 | exit(1); 140 | #endif 141 | #ifdef ARDUINO_SAMD_NANO_33_IOT 142 | if (feedbackPinNumber != 2 && 143 | feedbackPinNumber != 3 && 144 | feedbackPinNumber != 9 && 145 | feedbackPinNumber != 10 && 146 | feedbackPinNumber != 11 && 147 | feedbackPinNumber != 13 && 148 | feedbackPinNumber != 15 && // A1 149 | feedbackPinNumber != 19 && // A5 150 | feedbackPinNumber != 21) // A7 151 | exit(1); 152 | #endif 153 | } 154 | 155 | void FeedBackServo::handleFeedback() 156 | { 157 | // Interrupt Service Routine: triggered on pin CHANGE 158 | // Records timing only — actual decoding happens in main loop 159 | if (digitalRead(feedbackPinNumber_)) 160 | { 161 | rise_ = micros(); 162 | tLow_ = rise_ - fall_; 163 | } 164 | else 165 | { 166 | fall_ = micros(); 167 | tHigh_ = fall_ - rise_; 168 | feedbackUpdated_ = true; 169 | } 170 | } 171 | 172 | void FeedBackServo::updateAngleFromPWM() 173 | { 174 | if (!feedbackUpdated_) return; 175 | feedbackUpdated_ = false; 176 | 177 | int tCycle = tHigh_ + tLow_; 178 | if ((tCycle < 1000) || (tCycle > 1200)) 179 | return; 180 | 181 | float dc = (DUTY_SCALE * tHigh_) / (float)tCycle; 182 | float theta = ((dc - DC_MIN) * UNITS_FC) / (DC_MAX - DC_MIN); 183 | 184 | if (theta < 0.0) 185 | theta = 0.0; 186 | else if (theta > (UNITS_FC - 1.0)) 187 | theta = UNITS_FC - 1.0; 188 | 189 | if ((theta < Q2_MIN) && (thetaPre_ > Q3_MAX)) 190 | turns_++; 191 | else if ((thetaPre_ < Q2_MIN) && (theta > Q3_MAX)) 192 | turns_--; 193 | 194 | if (turns_ >= 0) 195 | angle_ = (turns_ * UNITS_FC) + theta; 196 | else if (turns_ < 0) 197 | angle_ = ((turns_ + 1) * UNITS_FC) - (UNITS_FC - theta); 198 | 199 | thetaPre_ = theta; 200 | } 201 | 202 | // ISR delegates 203 | void FeedBackServo::isr0() { if (instances[0]) instances[0]->handleFeedback(); } 204 | void FeedBackServo::isr1() { if (instances[1]) instances[1]->handleFeedback(); } 205 | void FeedBackServo::isr2() { if (instances[2]) instances[2]->handleFeedback(); } 206 | void FeedBackServo::isr3() { if (instances[3]) instances[3]->handleFeedback(); } 207 | void FeedBackServo::isr4() { if (instances[4]) instances[4]->handleFeedback(); } 208 | void FeedBackServo::isr5() { if (instances[5]) instances[5]->handleFeedback(); } 209 | --------------------------------------------------------------------------------