Skip to content

Commit 974118b

Browse files
OlafFiliesOlafFilies
committed
Add the XMC port for the Servo library
1 parent 629c08b commit 974118b

File tree

2 files changed

+381
-0
lines changed

2 files changed

+381
-0
lines changed

‎src/xmc/Servo.cpp

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
/******************************************************************************
2+
* The MIT License
3+
*
4+
* Copyright (c) 2010, LeafLabs, LLC.
5+
*
6+
* Permission is hereby granted, free of charge, to any person
7+
* obtaining a copy of this software and associated documentation
8+
* files (the "Software"), to deal in the Software without
9+
* restriction, including without limitation the rights to use, copy,
10+
* modify, merge, publish, distribute, sublicense, and/or sell copies
11+
* of the Software, and to permit persons to whom the Software is
12+
* furnished to do so, subject to the following conditions:
13+
*
14+
* The above copyright notice and this permission notice shall be
15+
* included in all copies or substantial portions of the Software.
16+
*
17+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18+
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19+
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20+
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21+
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22+
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23+
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
* SOFTWARE.
25+
*****************************************************************************/
26+
27+
#if defined(ARDUINO_ARCH_XMC)
28+
29+
#include "ServoTimers.h"
30+
31+
uint8_t _ServoCount = 1; // internal counter to check if max numbers of servos is reached
32+
static uint8_t _allowed[MAX_PWM_SERVOS] = ALLOWED_PINS; // internal array to check allowed pwm pins
33+
static uint8_t _servos[MAX_PWM_SERVOS]; // static array of used servo pins for checking
34+
35+
36+
/**
37+
* @brief None blocking wait loop.
38+
*
39+
* @param uS microseconds to wait
40+
*/
41+
static void _delayUs(unsigned long uS)
42+
{
43+
unsigned long time_now = micros();
44+
while (micros() < time_now + uS)
45+
;
46+
}
47+
48+
49+
Servo::Servo()
50+
{
51+
if (_ServoCount <= MAX_PWM_SERVOS )
52+
{
53+
this->servoIndex = _ServoCount++;
54+
55+
this->_minAngle = MIN_ANGLE;
56+
this->_maxAngle = MAX_ANGLE;
57+
this->_minPW = MIN_PULSE_WIDTH;
58+
this->_maxPW = MAX_PULSE_WIDTH;
59+
this->_pin = 0;
60+
this->_isActive = false;
61+
this->_pwm = 0;
62+
this->_deg = 0.0;
63+
}else{
64+
this->servoIndex = INVALID_SERVO;
65+
}
66+
}
67+
68+
uint8_t Servo::attach(uint8_t pin, uint16_t min, uint16_t max)
69+
{
70+
if (this->servoIndex <= MAX_PWM_SERVOS )
71+
{
72+
// validate selected pin
73+
bool pin_allowed = false;
74+
for( int i = 0; i < MAX_PWM_SERVOS; i++)
75+
{
76+
// check if pin already in use
77+
if ( _servos[i] == pin)
78+
return INVALID_SERVO;
79+
80+
// check if selected pin has a pwm unit on the used XMC board
81+
if ( _allowed[i] == pin)
82+
pin_allowed = true;
83+
}
84+
// return if pin is not found in allowed pin list
85+
if ( !pin_allowed )
86+
return INVALID_SERVO;
87+
88+
// Set min/max values according the input and check for absolute limits
89+
if (min < MIN_PULSE_CHECK)
90+
{
91+
this->_minAngle = constrain(min,MIN_ANGLE,MAX_ANGLE);
92+
this->_minPW = MIN_PULSE_WIDTH;
93+
} else {
94+
this->_minAngle = MIN_ANGLE; //TODO has to calculated
95+
this->_minPW = constrain(min,MIN_PULSE_WIDTH,MAX_PULSE_WIDTH);
96+
}
97+
98+
if (max < MIN_PULSE_CHECK)
99+
{
100+
this->_maxAngle = constrain(max,MIN_ANGLE,MAX_ANGLE);
101+
this->_maxPW = 2 * MAX_PULSE_WIDTH;
102+
} else {
103+
this->_maxAngle = MAX_ANGLE; //TODO has to calculated
104+
this->_maxPW = constrain(max,MIN_PULSE_WIDTH,MAX_PULSE_WIDTH);
105+
}
106+
107+
this->_pin = pin;
108+
this->_isActive = true;
109+
110+
setAnalogWriteFrequency(this->_pin, REFRESH_FREQUENCY);
111+
analogWriteResolution(ADC_RESOLUTION);
112+
113+
}
114+
115+
return this->servoIndex;
116+
}
117+
118+
119+
void Servo::detach()
120+
{
121+
this->servoIndex = _ServoCount--;
122+
123+
this->_minAngle = MIN_ANGLE;
124+
this->_maxAngle = MAX_ANGLE;
125+
this->_minPW = MIN_PULSE_WIDTH;
126+
this->_maxPW = MAX_PULSE_WIDTH;
127+
128+
this->_pin = 0;
129+
this->_isActive = false;
130+
this->_pwm = 0;
131+
this->_deg = 0.0;
132+
}
133+
134+
void Servo::write(int value)
135+
{
136+
if (value < MIN_PULSE_CHECK)
137+
{
138+
// angle must be inside the boundaries
139+
double angle = constrain(value, this->_minAngle, this->_maxAngle);
140+
double dutyCycle = ( 0.5 + ( angle / MAX_ANGLE ) * 2.0 ) * DUTYCYCLE_STEPS;
141+
142+
this->_deg = angle;
143+
this->_pwm = uint16_t(dutyCycle);
144+
145+
analogWrite(this->_pin, uint16_t(dutyCycle));
146+
_delayUs(50);
147+
} else {
148+
writeMicroseconds(value);
149+
}
150+
}
151+
152+
void Servo::writeMicroseconds(int value)
153+
{
154+
// value must be inside the boundaries
155+
double pw = constrain(value,this->_minPW, this->_maxPW);
156+
double dutyCycle = map(pw, MIN_PULSE_WIDTH,MAX_PULSE_WIDTH, 0.5 * DUTYCYCLE_STEPS, 2.5 * DUTYCYCLE_STEPS);
157+
158+
this->_deg = ( dutyCycle - DUTYCYCLE_STEPS * 0.5 ) * MAX_ANGLE / ( 2 * DUTYCYCLE_STEPS );
159+
this->_pwm = uint16_t(dutyCycle);
160+
161+
analogWrite(this->_pin, uint16_t(dutyCycle));
162+
_delayUs(50);
163+
}
164+
165+
#endif

‎src/xmc/ServoTimers.h

Lines changed: 216 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,216 @@
1+
/******************************************************************************
2+
* The MIT License
3+
*
4+
* Copyright (c) 2010, LeafLabs, LLC.
5+
*
6+
* Permission is hereby granted, free of charge, to any person
7+
* obtaining a copy of this software and associated documentation
8+
* files (the "Software"), to deal in the Software without
9+
* restriction, including without limitation the rights to use, copy,
10+
* modify, merge, publish, distribute, sublicense, and/or sell copies
11+
* of the Software, and to permit persons to whom the Software is
12+
* furnished to do so, subject to the following conditions:
13+
*
14+
* The above copyright notice and this permission notice shall be
15+
* included in all copies or substantial portions of the Software.
16+
*
17+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18+
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19+
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20+
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21+
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22+
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23+
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
* SOFTWARE.
25+
*****************************************************************************/
26+
27+
/*
28+
* @copyright Copyright (c) 2019-2020 Infineon Technologies AG
29+
*/
30+
#ifndef _SERVO_H_
31+
#define _SERVO_H_
32+
33+
#include <Arduino.h>
34+
#include "wiring_analog.h"
35+
36+
/*
37+
* Note on Arduino compatibility:
38+
*
39+
* In the Arduino implementation, PWM is done "by hand" in the sense
40+
* that timer channels are hijacked in groups and an ISR is set which
41+
* toggles Servo::attach()ed pins using digitalWrite().
42+
*
43+
* While this scheme allows any pin to drive a servo, it chews up
44+
* cycles and complicates the programmer's notion of when a particular
45+
* timer channel will be in use.
46+
*
47+
* This implementation only allows Servo instances to attach() to pins
48+
* that already have PWM unit associated with them, which drives the wave.
49+
*
50+
* While the Arduino implementation of attach() returns the affected channel,
51+
* this one returns the index number of the servo or an INVALID_SERVO = 255 in
52+
* case of an error.
53+
* The attach will check if a pin is already in use and if a pin has a PWM unit on
54+
* the selected XMC board, otherwise it returns an INVALID_SERVO.
55+
* This error handling is different than the original one from Arduino.
56+
*
57+
* Depending on the XMC type the number of possible PWM channels vary from 4 to 23
58+
* and may change with future version of the XMC series.
59+
*/
60+
61+
// Define the MAX_PWM_SERVOS number per XMC type and the allowed PWM pins on the selected XMC board
62+
#if defined(XMC1100_XMC2GO)
63+
#define MAX_PWM_SERVOS 4
64+
#define ALLOWED_PINS {1, 2, 3, 8,}
65+
#elif defined(XMC1100_Boot_Kit)
66+
#define MAX_PWM_SERVOS 6
67+
#define ALLOWED_PINS { 3,4,6,9,10,11 }
68+
#elif defined(XMC1300_Boot_Kit)
69+
#define MAX_PWM_SERVOS 4
70+
#define ALLOWED_PINS { 26,31,32,33 }
71+
#elif defined(XMC1400_Arduino_Kit)
72+
#define MAX_PWM_SERVOS 6
73+
#define ALLOWED_PINS { 3,4,6,9,10,11 }
74+
#elif defined(XMC4200_Platform2GO)
75+
#define MAX_PWM_SERVOS 7
76+
#define ALLOWED_PINS { 3,5,6,9,22,23,24 }
77+
#elif defined(XMC4400_Platform2GO)
78+
#define MAX_PWM_SERVOS 15
79+
#define ALLOWED_PINS { 3,5,6,9,10,14,25,26,27,28,29,30,45,48,67 }
80+
#elif defined(XMC4700_Relax_Kit)
81+
#define MAX_PWM_SERVOS 23
82+
#define ALLOWED_PINS { 3,5,6,9,10,11,34,36,37,51,61,62,66,70,76,77,79,80,81,88,89,93,94 }
83+
#else
84+
#error "Not a supported XMC Board"
85+
#endif
86+
87+
#define MIN_ANGLE 0 // the minimal angle in degree
88+
#define MAX_ANGLE 180 // the maximal angle in degree
89+
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo in microseconds
90+
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo in microseconds
91+
92+
#define MIN_PULSE_CHECK 500 // border with below = angle and above = pulse width
93+
#define REFRESH_FREQUENCY 50u // the refresh frequency on analog pins
94+
#define REFRESH_TIME 20.0 // the PWM refresh frequency for the servo motor
95+
#define DUTYCYCLE_STEPS 65536.0 / REFRESH_TIME // the number of duty cycle steps during one refresh period
96+
#define ADC_RESOLUTION 16 // the resolution of the adc during analog write
97+
98+
#define INVALID_SERVO 255 // flag indicating an invalid servo index
99+
100+
/** Class for interfacing with RC servomotors. */
101+
class Servo
102+
{
103+
public:
104+
/**
105+
* @brief Construct a new Servo instance.
106+
*
107+
* The new instance will not be attached to any pin, but only PWM capable pins will run.
108+
* see pin list above.
109+
*/
110+
Servo();
111+
112+
/**
113+
* @brief Associate this instance with a servomotor whose input is
114+
* connected to pin.
115+
*
116+
* If this instance is already attached to a pin, it will be
117+
* detached before being attached to the new pin.
118+
* If the pin is not allowed for running PWM or the max number of
119+
* PWM channels on the XMC board is reached it will return
120+
* with an INVALID_SERVO, otherwise with the servoIndex number.
121+
*
122+
* @param pin Pin connected to the servo pulse wave input. This
123+
* pin must be capable of PWM output.
124+
*
125+
* @param min If this value is below MIN_PULSE_CHECK it will be associated
126+
* with an angle in degree. Otherwise it will be the minimum
127+
* pulse width.
128+
* min as an angle must be between MIN_ANGLE < angle < MAX_ANGLE
129+
* with default as MIN_ANGLE
130+
* min as a pulse width must be between MIN_PULSE_WIDTH < pwm < MAX_PULSE_WIDTH
131+
* with a default as MIN_PULSE_WIDTH
132+
*
133+
* @param max If this value is below MIN_PULSE_CHECK it will be associated
134+
* with an angle in degree. Otherwise it will be the maximum
135+
* pulse width.
136+
* max as an angle must be between MIN_ANGLE < angle < MAX_ANGLE
137+
* with default as MAX_ANGLE
138+
* max as a pulse width must be between MIN_PULSE_WIDTH < pwm < MAX_PULSE_WIDTH
139+
* with a default as MAX_PULSE_WIDTH
140+
*
141+
* @return servoIndex number or INVALID_SERVO = 255 in case of an error
142+
*/
143+
uint8_t attach(uint8_t pin, uint16_t min = MIN_ANGLE, uint16_t max = MAX_ANGLE);
144+
145+
146+
/**
147+
* @brief Stop driving the servo pulse train.
148+
*
149+
* If not currently attached to a motor, this function has no effect.
150+
*
151+
* @return true if this call did anything, false otherwise.
152+
*/
153+
void detach();
154+
155+
/**
156+
* @brief Set the servomotor target angle by recalculating the duty cycle
157+
* for XMC PWM settings.
158+
*
159+
* @param value Target angle, in degrees. If the target angle is
160+
* outside the range specified at attach(), it
161+
* will be clamped to lie in that range.
162+
*
163+
* @see Servo::attach()
164+
*/
165+
void write(int value);
166+
167+
/**
168+
* @brief Set the pulse width, in microseconds by recalculating it for the
169+
* XMC PWM settings. It also calculates the angle from the pwm value.
170+
*
171+
* @param value Pulse width to send to the servomotor, in
172+
* microseconds. If outside of the range
173+
* specified at attach() time, it is clamped to
174+
* lie in that range.
175+
*
176+
* @see Servo::attach()
177+
*/
178+
void writeMicroseconds(int value);
179+
180+
/**
181+
* returns the current value in degree as an angle between 0 and 189 degrees
182+
*
183+
* @see Servo::attach()
184+
*/
185+
int read() const { return uint16_t(this->_deg); }
186+
187+
/**
188+
* returns the current pwm value in microseconds.
189+
*
190+
* @see Servo::attach()
191+
*/
192+
int readMicroseconds() const { return uint16_t(this->_pwm); }
193+
194+
/**
195+
* @brief Check if this instance is attached to a servo.
196+
* @return true if this instance is attached to a servo, false otherwise.
197+
* @see Servo::attachedPin()
198+
*/
199+
bool attached() const { return this->_isActive; }
200+
201+
private:
202+
uint16_t _minPW; // the initial minPulseWidth, if not set than MIN_PULSE_WIDTH
203+
uint16_t _maxPW; // the initial maxPulseWidth, if not set than MAX_PULSE_WIDTH
204+
int16_t _minAngle; // the initial minAngle, if not set than MIN_ANGLE
205+
int16_t _maxAngle; // the initial maxAngle, if not set than MAX_ANGLE
206+
int16_t _pin; // attached arduino pin number
207+
double _deg; // actual angle in degree
208+
double _pwm; // actual pwm signal in microseconds
209+
uint8_t _isActive; // true if this pin is active, otherwise false
210+
211+
uint8_t servoIndex; // the actual number of Servos attached to this library
212+
213+
214+
};
215+
216+
#endif /* _SERVO_H_ */

0 commit comments

Comments
 (0)