Skip to content

Commit 758e740

Browse files
HyodaKazuakiHyodaKazuaki
authored
Merge pull request #3 from HyodaKazuaki/feature/support-arduino-due
Add Arduino Due support close #2
2 parents f235b66 + 2f36fae commit 758e740

File tree

4 files changed

+85
-109
lines changed

4 files changed

+85
-109
lines changed

‎SUPPORTED.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,4 +33,4 @@ If you have successfully used this library with an unconfirmed board, please let
3333
| Arduino Industrial 101 | △ |
3434
| Linino One | △ |
3535
| Arduino Uno WiFi | △ |
36-
| Arduino Due | ╳ |
36+
| Arduino Due | △ |

‎library.properties

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
name= Parallax FeedBack360 Servo Control Library
2-
version=1.0.0
2+
version=1.1.2
33
author=Hyoda Kazuaki<[email protected]>
44
maintainer=Hyoda Kazuaki<[email protected]>
55
sentence=A library that makes control Parallax FeedBack360 servo easy.
66
paragraph=You can control servo to degree of rotation what you want with this library. Of course, you can read servo's degree of rotation.
77
category=Device Control
88
url=https://github.com/HyodaKazuaki/FeedBack360-Servo-Control-Library-4-Arduino
9-
architectures=avr
9+
architectures=avr,sam

‎src/FeedBackServo.cpp

Lines changed: 56 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -8,53 +8,63 @@
88
#include <Servo.h>
99
#include "FeedBackServo.h"
1010

11-
static Servo FeedBackServo::Parallax;
12-
static byte FeedBackServo::feedbackPinNumber = 2;
13-
static volatile int FeedBackServo::angle;
14-
static float FeedBackServo::thetaPre;
15-
static unsigned int FeedBackServo::tHigh, FeedBackServo::tLow;
16-
static unsigned long FeedBackServo::rise, FeedBackServo::fall;
17-
static int FeedBackServo::turns = 0;
18-
static float FeedBackServo::Kp = 1.0;
19-
20-
FeedBackServo::FeedBackServo(byte _feedbackPinNumber = 2)
11+
Servo FeedBackServo::Parallax;
12+
byte FeedBackServo::feedbackPinNumber = 2;
13+
volatile int FeedBackServo::angle;
14+
float FeedBackServo::thetaPre;
15+
unsigned int FeedBackServo::tHigh, FeedBackServo::tLow;
16+
unsigned long FeedBackServo::rise, FeedBackServo::fall;
17+
int FeedBackServo::turns = 0;
18+
float FeedBackServo::Kp = 1.0;
19+
const int FeedBackServo::unitsFC = 360;
20+
const float FeedBackServo::dcMin = 0.029;
21+
const float FeedBackServo::dcMax = 0.971;
22+
const int FeedBackServo::dutyScale = 1;
23+
const int FeedBackServo::q2min = unitsFC / 4;
24+
const int FeedBackServo::q3max = q2min * 3;
25+
26+
FeedBackServo::FeedBackServo(byte _feedbackPinNumber)
2127
{
2228
// feedback pin number validation
2329
pinCheck(_feedbackPinNumber);
2430
feedbackPinNumber = _feedbackPinNumber;
2531

26-
// convert feedback pin number for use on attachInterrupt function
27-
byte internalPinNumber = convertFeedbackPin();
32+
// convert feedback pin number to interrupt number for use on attachInterrupt function
33+
byte internalPinNumber = digitalPinToInterrupt(feedbackPinNumber);
2834

2935
attachInterrupt(internalPinNumber, feedback, CHANGE);
3036
}
3137

32-
void FeedBackServo::setServoControl(byte servoPinNumber = 3)
38+
void FeedBackServo::setServoControl(byte servoPinNumber)
3339
{
3440
// Servo control pin attach
3541
Parallax.attach(servoPinNumber);
3642
}
3743

38-
void FeedBackServo::setKp(float _Kp = 1.0) {
44+
void FeedBackServo::setKp(float _Kp)
45+
{
3946
FeedBackServo::Kp = _Kp;
4047
}
4148

42-
void FeedBackServo::rotate(int degree, int threshold = 4)
49+
void FeedBackServo::rotate(int degree, int threshold)
4350
{
4451
float output, offset, value;
45-
for(int errorAngle = degree - angle; abs(errorAngle) > threshold; errorAngle = degree - angle) {
52+
for (int errorAngle = degree - angle;
53+
abs(errorAngle) > threshold;
54+
errorAngle = degree - angle)
55+
{
4656
output = errorAngle * Kp;
47-
if(output > 200.0)
57+
if (output > 200.0)
4858
output = 200.0;
49-
if(output < -200.0)
59+
if (output < -200.0)
5060
output = -200.0;
51-
if(errorAngle > 0)
61+
if (errorAngle > 0)
5262
offset = 30.0;
53-
else if(errorAngle < 0)
63+
else if (errorAngle < 0)
5464
offset = -30.0;
5565
else
5666
offset = 0.0;
57-
67+
5868
value = output + offset;
5969
Parallax.writeMicroseconds(1490 - value);
6070
}
@@ -68,89 +78,55 @@ int FeedBackServo::Angle()
6878

6979
void FeedBackServo::pinCheck(byte _feedbackPinNumber)
7080
{
71-
// Check pin number
72-
#ifdef ARDUINO_AVR_UNO
73-
if(_feedbackPinNumber != 2 && _feedbackPinNumber != 3)
81+
// Check pin number
82+
#ifdef ARDUINO_AVR_UNO
83+
if (_feedbackPinNumber != 2 &&
84+
_feedbackPinNumber != 3)
7485
exit(1);
75-
#endif
76-
#ifdef ARDUINO_AVR_LEONARDO
77-
if(_feedbackPinNumber != 0 && _feedbackPinNumber != 1 && _feedbackPinNumber != 2 && _feedbackPinNumber != 3 && _feedbackPinNumber != 7)
86+
#endif
87+
#ifdef ARDUINO_AVR_LEONARDO
88+
if (_feedbackPinNumber != 0 &&
89+
_feedbackPinNumber != 1 &&
90+
_feedbackPinNumber != 2 &&
91+
_feedbackPinNumber != 3 &&
92+
_feedbackPinNumber != 7)
7893
exit(1);
79-
#endif
94+
#endif
8095
}
8196

82-
byte FeedBackServo::convertFeedbackPin()
97+
void FeedBackServo::feedback()
8398
{
84-
byte internalPinNumber;
85-
#ifdef ARDUINO_AVR_UNO
86-
switch (feedbackPinNumber)
99+
if (digitalRead(feedbackPinNumber))
87100
{
88-
case 2:
89-
internalPinNumber = 0;
90-
break;
91-
case 3:
92-
internalPinNumber = 1;
93-
break;
94-
default:
95-
internalPinNumber = 0;
96-
break;
97-
}
98-
#endif
99-
#ifdef ARDUINO_AVR_LEONARDO
100-
switch (feedbackPinNumber)
101-
{
102-
case 0:
103-
internalPinNumber = 2;
104-
break;
105-
case 1:
106-
internalPinNumber = 3;
107-
break;
108-
case 2:
109-
internalPinNumber = 1;
110-
break;
111-
case 3:
112-
internalPinNumber = 0;
113-
break;
114-
case 7:
115-
internalPinNumber = 4;
116-
break;
117-
default:
118-
internalPinNumber = 0;
119-
break;
120-
}
121-
#endif
122-
return internalPinNumber;
123-
}
124-
125-
static void FeedBackServo::feedback() {
126-
if(digitalRead(feedbackPinNumber)) {
127101
rise = micros();
128102
tLow = rise - fall;
129103

130104
int tCycle = tHigh + tLow;
131-
if((tCycle < 1000) || (tCycle > 1200))
105+
if ((tCycle < 1000) || (tCycle > 1200))
132106
return;
133-
107+
134108
float dc = (dutyScale * tHigh) / (float)tCycle;
135109
float theta = ((dc - dcMin) * unitsFC) / (dcMax - dcMin);
136110

137-
if(theta < 0.0)
111+
if (theta < 0.0)
138112
theta = 0.0;
139-
else if(theta > (unitsFC - 1.0))
113+
else if (theta > (unitsFC - 1.0))
140114
theta = unitsFC - 1.0;
141115

142-
if((theta < q2min) && (thetaPre > q3max))
116+
if ((theta < q2min) && (thetaPre > q3max))
143117
turns++;
144-
else if((thetaPre < q2min) && (theta > q3max))
118+
else if ((thetaPre < q2min) && (theta > q3max))
145119
turns--;
146120

147-
if(turns >= 0)
121+
if (turns >= 0)
148122
angle = (turns * unitsFC) + theta;
149-
else if(turns < 0)
123+
else if (turns < 0)
150124
angle = ((turns + 1) * unitsFC) - (unitsFC - theta);
151125

152126
thetaPre = theta;
153-
} else {
127+
}
128+
else
129+
{
154130
fall = micros();
155131
tHigh = fall - rise;
156132
}

‎src/FeedBackServo.h

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -3,32 +3,32 @@
33
#include <Arduino.h>
44
#include <Servo.h>
55

6-
class FeedBackServo {
7-
public:
8-
FeedBackServo(byte feedbackPinNumber = 2);
9-
void setServoControl(byte servoPinNumber = 3);
10-
void setKp(float _Kp = 1.0);
11-
void rotate(int degree, int threshold = 4);
12-
int Angle();
13-
14-
private:
15-
void pinCheck(byte pinNumber);
16-
byte convertFeedbackPin();
17-
void static feedback();
18-
static Servo Parallax;
19-
static byte feedbackPinNumber;
20-
static volatile int angle;
21-
static float thetaPre;
22-
static unsigned int tHigh, tLow;
23-
static unsigned long rise, fall;
24-
static int turns;
25-
static float Kp;
26-
static const int unitsFC = 360;
27-
static const float dcMin = 0.029;
28-
static const float dcMax = 0.971;
29-
static const int dutyScale = 1;
30-
static const int q2min = unitsFC / 4;
31-
static const int q3max = q2min * 3;
6+
class FeedBackServo
7+
{
8+
public:
9+
FeedBackServo(byte feedbackPinNumber = 2);
10+
void setServoControl(byte servoPinNumber = 3);
11+
void setKp(float _Kp = 1.0);
12+
void rotate(int degree, int threshold = 4);
13+
int Angle();
14+
15+
private:
16+
void pinCheck(byte pinNumber);
17+
void static feedback();
18+
static Servo Parallax;
19+
static byte feedbackPinNumber;
20+
static volatile int angle;
21+
static float thetaPre;
22+
static unsigned int tHigh, tLow;
23+
static unsigned long rise, fall;
24+
static int turns;
25+
static float Kp;
26+
static const int unitsFC;
27+
static const float dcMin;
28+
static const float dcMax;
29+
static const int dutyScale;
30+
static const int q2min;
31+
static const int q3max;
3232
};
3333

3434
#endif

0 commit comments

Comments
 (0)