Skip to content

Commit 7595b87

Browse files
IFX-AnushaIFX-Anusha
committed
src/psoc: Add psoc6 port for Servo Library.
Signed-off-by: IFX-Anusha <[email protected]>
1 parent 2832d5c commit 7595b87

File tree

4 files changed

+354
-3
lines changed

4 files changed

+354
-3
lines changed

‎library.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,4 @@ sentence=Allows Arduino boards to control a variety of servo motors.
66
paragraph=This library can control a great number of servos.<br />It makes careful use of timers: the library can control 12 servos using only 1 timer.<br />On the Arduino Due you can control up to 60 servos.
77
category=Device Control
88
url=https://www.arduino.cc/reference/en/libraries/servo/
9-
architectures=avr,megaavr,sam,samd,nrf52,stm32f4,mbed,mbed_nano,mbed_portenta,mbed_rp2040,renesas,renesas_portenta,renesas_uno
9+
architectures=avr,megaavr,sam,samd,nrf52,stm32f4,mbed,mbed_nano,mbed_portenta,mbed_rp2040,renesas,renesas_portenta,renesas_uno,psoc6

‎src/Servo.h‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -79,8 +79,10 @@
7979
#include "xmc/ServoTimers.h"
8080
#elif defined(ARDUINO_ARCH_ESP32)
8181
#include "esp32/ServoTimers.h"
82+
#elif defined(ARDUINO_ARCH_PSOC6)
83+
#include "psoc6/ServoTimers.h"
8284
#else
83-
#error "This library only supports boards with an AVR, SAM, SAMD, NRF52, STM32F4, Renesas, XMC or ESP32 processor."
85+
#error "This library only supports boards with an AVR, SAM, SAMD, NRF52, STM32F4, Renesas, XMC, ESP32 or PSOC6 processor."
8486
#endif
8587

8688
#define Servo_VERSION 2 // software version of this library
@@ -95,7 +97,7 @@
9597

9698
#define INVALID_SERVO 255 // flag indicating an invalid servo index
9799

98-
#if !defined(ARDUINO_ARCH_STM32F4) && !defined(ARDUINO_ARCH_XMC)
100+
#if !defined(ARDUINO_ARCH_STM32F4) && !defined(ARDUINO_ARCH_XMC) &&!defined(ARDUINO_ARCH_PSOC6)
99101

100102
typedef struct {
101103
uint8_t nbr :6 ; // a pin number from 0 to 63

‎src/psoc6/Servo.cpp‎

Lines changed: 180 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
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_PSOC6)
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+
Servo::Servo()
49+
{
50+
if(_ServoCount <= MAX_PWM_SERVOS)
51+
{
52+
this->servoIndex = _ServoCount++;
53+
54+
// Initialize the servo with default values
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+
64+
for(int i = 0; i < MAX_PWM_SERVOS; i++)
65+
{
66+
_servos[i] = 0xFF; // Initialize all servo pins to unused state
67+
}
68+
}
69+
else
70+
{
71+
this->servoIndex = INVALID_SERVO; // No more servos can be attached
72+
}
73+
}
74+
75+
uint8_t Servo::attach(uint8_t pin, uint16_t min, uint16_t max)
76+
{
77+
if(this->servoIndex <= MAX_PWM_SERVOS)
78+
{
79+
// Validate the pin
80+
bool pin_allowed = false;
81+
for(int i = 0; i < MAX_PWM_SERVOS; i++)
82+
{
83+
// Check if pin already in use
84+
if(_servos[i] == pin)
85+
{
86+
return INVALID_SERVO;
87+
}
88+
// Check if selected pin has a PWM unit on the used PSOC6 board
89+
if(_allowed[i] == pin)
90+
pin_allowed = true;
91+
}
92+
93+
// Return if pin is not found in allowed pin list
94+
if(!pin_allowed)
95+
return INVALID_SERVO;
96+
97+
// Set min/max values according to input and check for absolute limits
98+
if(min < MIN_PULSE_CHECK)
99+
{
100+
this->_minAngle = constrain(min, MIN_ANGLE, MAX_ANGLE);
101+
this->_minPW = MIN_PULSE_WIDTH;
102+
}
103+
else
104+
{
105+
this->_minAngle = MIN_ANGLE; // TODO: has to be calculated
106+
this->_minPW = constrain(min, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
107+
}
108+
109+
if(max < MIN_PULSE_CHECK)
110+
{
111+
this->_maxAngle = constrain(max, MIN_ANGLE, MAX_ANGLE);
112+
this->_maxPW = 2 * MAX_PULSE_WIDTH;
113+
}
114+
else
115+
{
116+
this->_maxAngle = MAX_ANGLE; // TODO: has to be calculated
117+
this->_maxPW = constrain(max, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
118+
}
119+
120+
this->_pin = pin;
121+
this->_isActive = true;
122+
_servos[this->servoIndex - 1] = pin; // Mark the pin as used
123+
analogWriteResolution(RESOLUTION);
124+
setAnalogWriteFrequency(this->_pin, REFRESH_FREQUENCY); // Set the default frequency for the pin
125+
126+
}
127+
return this->servoIndex;
128+
}
129+
130+
void Servo::detach()
131+
{
132+
this->servoIndex = _ServoCount--;
133+
134+
this->_minAngle = MIN_ANGLE;
135+
this->_maxAngle = MAX_ANGLE;
136+
this->_minPW = MIN_PULSE_WIDTH;
137+
this->_maxPW = MAX_PULSE_WIDTH;
138+
139+
this->_pin = 0;
140+
this->_isActive = false;
141+
this->_pwm = 0;
142+
this->_deg = 0.0;
143+
_servos[this->servoIndex - 1] = 0xFF; // Mark the pin as unused
144+
}
145+
146+
void Servo::write(int value)
147+
{
148+
if (value < MIN_PULSE_CHECK)
149+
{
150+
// angle must be inside the boundaries
151+
double angle = constrain(value, this->_minAngle, this->_maxAngle);
152+
double dutyCycle = ( 0.5 + ( angle / MAX_ANGLE ) * 2.0 ) * DUTYCYCLE_STEPS;
153+
154+
this->_deg = angle;
155+
this->_pwm = uint16_t(dutyCycle);
156+
157+
analogWrite(this->_pin, uint16_t(dutyCycle));
158+
_delayUs(50);
159+
} else {
160+
writeMicroseconds(value);
161+
}
162+
}
163+
164+
void Servo::writeMicroseconds(int value)
165+
{
166+
// value must be inside the boundaries
167+
double pw = constrain(value,this->_minPW, this->_maxPW);
168+
double dutyCycle = map(pw, MIN_PULSE_WIDTH,MAX_PULSE_WIDTH, 0.5 * DUTYCYCLE_STEPS, 2.5 * DUTYCYCLE_STEPS);
169+
170+
this->_deg = ( dutyCycle - DUTYCYCLE_STEPS * 0.5 ) * MAX_ANGLE / ( 2 * DUTYCYCLE_STEPS );
171+
this->_pwm = uint16_t(dutyCycle);
172+
analogWrite(this->_pin, uint16_t(dutyCycle));
173+
174+
_delayUs(50);
175+
}
176+
#endif
177+
178+
179+
180+

‎src/psoc6/ServoTimers.h‎

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
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) 2025-2026 Infineon Technologies AG
29+
*/
30+
31+
#ifndef __SERVO_TIMERS_H__
32+
#define __SERVO_TIMERS_H__
33+
34+
#include <Arduino.h>
35+
36+
#define MAX_PWM_SERVOS 28
37+
#define ALLOWED_PINS {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30}
38+
39+
40+
#define MIN_ANGLE 0 // the minimal angle in degree
41+
#define MAX_ANGLE 180 // the maximal angle in degree
42+
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo in microseconds
43+
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo in microseconds
44+
45+
#define MIN_PULSE_CHECK 500 // border with below = angle and above = pulse width
46+
#define REFRESH_FREQUENCY 50u // the refresh frequency on analog pins
47+
#define REFRESH_TIME 20.0 // the PWM refresh frequency for the servo motor
48+
#define DUTYCYCLE_STEPS 65536.0 / REFRESH_TIME // the number of duty cycle steps during one refresh period
49+
#define RESOLUTION 16 // the resolution of the adc during analog write
50+
51+
#define INVALID_SERVO 255 // flag indicating an invalid servo index
52+
53+
/** Class for interfacing with RC servomotors. */
54+
class Servo
55+
{
56+
public:
57+
/**
58+
* @brief Construct a new Servo instance.
59+
*
60+
* The new instance will not be attached to any pin, but only PWM capable pins will run.
61+
* see pin list above.
62+
*/
63+
Servo();
64+
65+
/**
66+
* @brief Associate this instance with a servomotor whose input is
67+
* connected to pin.
68+
*
69+
* If this instance is already attached to a pin, it will be
70+
* detached before being attached to the new pin.
71+
* If the pin is not allowed for running PWM or the max number of
72+
* PWM channels on the PSOC6 board is reached it will return
73+
* with an INVALID_SERVO, otherwise with the servoIndex number.
74+
*
75+
* @param pin Pin connected to the servo pulse wave input. This
76+
* pin must be capable of PWM output.
77+
*
78+
* @param min If this value is below MIN_PULSE_CHECK it will be associated
79+
* with an angle in degree. Otherwise it will be the minimum
80+
* pulse width.
81+
* min as an angle must be between MIN_ANGLE < angle < MAX_ANGLE
82+
* with default as MIN_ANGLE
83+
* min as a pulse width must be between MIN_PULSE_WIDTH < pwm < MAX_PULSE_WIDTH
84+
* with a default as MIN_PULSE_WIDTH
85+
*
86+
* @param max If this value is below MIN_PULSE_CHECK it will be associated
87+
* with an angle in degree. Otherwise it will be the maximum
88+
* pulse width.
89+
* max as an angle must be between MIN_ANGLE < angle < MAX_ANGLE
90+
* with default as MAX_ANGLE
91+
* max as a pulse width must be between MIN_PULSE_WIDTH < pwm < MAX_PULSE_WIDTH
92+
* with a default as MAX_PULSE_WIDTH
93+
*
94+
* @return servoIndex number or INVALID_SERVO = 255 in case of an error
95+
*/
96+
uint8_t attach(uint8_t pin, uint16_t min = MIN_ANGLE, uint16_t max = MAX_ANGLE);
97+
98+
99+
/**
100+
* @brief Stop driving the servo pulse train.
101+
*
102+
* If not currently attached to a motor, this function has no effect.
103+
*
104+
* @return true if this call did anything, false otherwise.
105+
*/
106+
void detach();
107+
108+
/**
109+
* @brief Set the servomotor target angle by recalculating the duty cycle
110+
* for PSOC6 PWM settings.
111+
*
112+
* @param value Target angle, in degrees. If the target angle is
113+
* outside the range specified at attach(), it
114+
* will be clamped to lie in that range.
115+
*
116+
* @see Servo::attach()
117+
*/
118+
void write(int value);
119+
120+
/**
121+
* @brief Set the pulse width, in microseconds by recalculating it for the
122+
* PSOC6 PWM settings. It also calculates the angle from the pwm value.
123+
*
124+
* @param value Pulse width to send to the servomotor, in
125+
* microseconds. If outside of the range
126+
* specified at attach() time, it is clamped to
127+
* lie in that range.
128+
*
129+
* @see Servo::attach()
130+
*/
131+
void writeMicroseconds(int value);
132+
133+
/**
134+
* returns the current value in degree as an angle between 0 and 189 degrees
135+
*
136+
* @see Servo::attach()
137+
*/
138+
int read() const { return uint16_t(this->_deg); }
139+
140+
/**
141+
* returns the current pwm value in microseconds.
142+
*
143+
* @see Servo::attach()
144+
*/
145+
int readMicroseconds() const { return uint16_t(this->_pwm); }
146+
147+
/**
148+
* @brief Check if this instance is attached to a servo.
149+
* @return true if this instance is attached to a servo, false otherwise.
150+
* @see Servo::attachedPin()
151+
*/
152+
bool attached() const { return this->_isActive; }
153+
154+
private:
155+
uint16_t _minPW; // the initial minPulseWidth, if not set than MIN_PULSE_WIDTH
156+
uint16_t _maxPW; // the initial maxPulseWidth, if not set than MAX_PULSE_WIDTH
157+
int16_t _minAngle; // the initial minAngle, if not set than MIN_ANGLE
158+
int16_t _maxAngle; // the initial maxAngle, if not set than MAX_ANGLE
159+
int16_t _pin; // attached arduino pin number
160+
double _deg; // actual angle in degree
161+
double _pwm; // actual pwm signal in microseconds
162+
uint8_t _isActive; // true if this pin is active, otherwise false
163+
164+
uint8_t servoIndex; // the actual number of Servos attached to this library
165+
166+
167+
};
168+
169+
#endif

0 commit comments

Comments
 (0)