Skip to content
This repository was archived by the owner on Mar 31, 2023. It is now read-only.

Commit 41414db

Browse files
ridhaosWi6labsVVESTM
ridhaosWi6labs
authored and
VVESTM
committed
Update Servo library for STM8 architecture
1 parent cf7799b commit 41414db

File tree

11 files changed

+1339
-218
lines changed

11 files changed

+1339
-218
lines changed

‎libraries/Servo/library.properties

+1-1
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,4 @@ sentence=Allows Arduino/Genuino 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.<br />
77
category=Device Control
88
url=http://www.arduino.cc/en/Reference/Servo
9-
architectures=avr,sam,samd,nrf52,stm32f4,stm32
9+
architectures=avr,sam,samd,nrf52,stm32f4,stm8

‎libraries/Servo/src/Servo.h

+6-13
Original file line numberDiff line numberDiff line change
@@ -65,30 +65,24 @@
6565
#include "sam/ServoTimers.h"
6666
#elif defined(ARDUINO_ARCH_SAMD)
6767
#include "samd/ServoTimers.h"
68-
#elif defined(ARDUINO_ARCH_STM32F4)
69-
#include "stm32f4/ServoTimers.h"
70-
#elif defined(ARDUINO_ARCH_NRF52)
71-
#include "nrf52/ServoTimers.h"
72-
#elif defined(ARDUINO_ARCH_STM32)
73-
#include "stm32/ServoTimers.h"
68+
#elif defined(ARDUINO_ARCH_STM8)
69+
#include "stm8/ServoTimers.h"
7470
#else
75-
#error "This library only supports boards with an AVR, SAM, SAMD, NRF52, STM32F4 or STM32 processor."
71+
#error "This library only supports boards with an AVR, SAM, SAMD, NRF52 or STM8 processor."
7672
#endif
7773

7874
#define Servo_VERSION 2 // software version of this library
7975

80-
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
81-
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
76+
#define MIN_PULSE_WIDTH 500 // the shortest pulse sent to a servo
77+
#define MAX_PULSE_WIDTH 2000 // the longest pulse sent to a servo
8278
#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
83-
#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
79+
#define REFRESH_INTERVAL 285000 // minumim time to refresh servos in microseconds
8480

8581
#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
8682
#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
8783

8884
#define INVALID_SERVO 255 // flag indicating an invalid servo index
8985

90-
#if !defined(ARDUINO_ARCH_STM32F4)
91-
9286
typedef struct {
9387
uint8_t nbr :6 ; // a pin number from 0 to 63
9488
uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false
@@ -118,4 +112,3 @@ class Servo
118112
};
119113

120114
#endif
121-
#endif

‎libraries/Servo/src/avr/Servo.cpp

+317
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,317 @@
1+
/*
2+
Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
3+
Copyright (c) 2009 Michael Margolis. All right reserved.
4+
5+
This library is free software; you can redistribute it and/or
6+
modify it under the terms of the GNU Lesser General Public
7+
License as published by the Free Software Foundation; either
8+
version 2.1 of the License, or (at your option) any later version.
9+
10+
This library is distributed in the hope that it will be useful,
11+
but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13+
Lesser General Public License for more details.
14+
15+
You should have received a copy of the GNU Lesser General Public
16+
License along with this library; if not, write to the Free Software
17+
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
18+
*/
19+
20+
#if defined(ARDUINO_ARCH_AVR)
21+
22+
#include <avr/interrupt.h>
23+
#include <Arduino.h>
24+
25+
#include "Servo.h"
26+
27+
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
28+
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
29+
30+
31+
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
32+
33+
//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
34+
35+
static servo_t servos[MAX_SERVOS]; // static array of servo structures
36+
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
37+
38+
uint8_t ServoCount = 0; // the total number of attached servos
39+
40+
41+
// convenience macros
42+
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
43+
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
44+
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
45+
#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
46+
47+
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
48+
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
49+
50+
/************ static functions common to all instances ***********************/
51+
52+
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
53+
{
54+
if( Channel[timer] < 0 )
55+
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
56+
else{
57+
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
58+
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
59+
}
60+
61+
Channel[timer]++; // increment to the next channel
62+
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
63+
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
64+
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
65+
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
66+
}
67+
else {
68+
// finished all channels so wait for the refresh period to expire before starting over
69+
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
70+
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
71+
else
72+
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
73+
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
74+
}
75+
}
76+
77+
#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
78+
// Interrupt handlers for Arduino
79+
#if defined(_useTimer1)
80+
SIGNAL (TIMER1_COMPA_vect)
81+
{
82+
handle_interrupts(_timer1, &TCNT1, &OCR1A);
83+
}
84+
#endif
85+
86+
#if defined(_useTimer3)
87+
SIGNAL (TIMER3_COMPA_vect)
88+
{
89+
handle_interrupts(_timer3, &TCNT3, &OCR3A);
90+
}
91+
#endif
92+
93+
#if defined(_useTimer4)
94+
SIGNAL (TIMER4_COMPA_vect)
95+
{
96+
handle_interrupts(_timer4, &TCNT4, &OCR4A);
97+
}
98+
#endif
99+
100+
#if defined(_useTimer5)
101+
SIGNAL (TIMER5_COMPA_vect)
102+
{
103+
handle_interrupts(_timer5, &TCNT5, &OCR5A);
104+
}
105+
#endif
106+
107+
#elif defined WIRING
108+
// Interrupt handlers for Wiring
109+
#if defined(_useTimer1)
110+
void Timer1Service()
111+
{
112+
handle_interrupts(_timer1, &TCNT1, &OCR1A);
113+
}
114+
#endif
115+
#if defined(_useTimer3)
116+
void Timer3Service()
117+
{
118+
handle_interrupts(_timer3, &TCNT3, &OCR3A);
119+
}
120+
#endif
121+
#endif
122+
123+
124+
static void initISR(timer16_Sequence_t timer)
125+
{
126+
#if defined (_useTimer1)
127+
if(timer == _timer1) {
128+
TCCR1A = 0; // normal counting mode
129+
TCCR1B = _BV(CS11); // set prescaler of 8
130+
TCNT1 = 0; // clear the timer count
131+
#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
132+
TIFR |= _BV(OCF1A); // clear any pending interrupts;
133+
TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
134+
#else
135+
// here if not ATmega8 or ATmega128
136+
TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
137+
TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
138+
#endif
139+
#if defined(WIRING)
140+
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
141+
#endif
142+
}
143+
#endif
144+
145+
#if defined (_useTimer3)
146+
if(timer == _timer3) {
147+
TCCR3A = 0; // normal counting mode
148+
TCCR3B = _BV(CS31); // set prescaler of 8
149+
TCNT3 = 0; // clear the timer count
150+
#if defined(__AVR_ATmega128__)
151+
TIFR |= _BV(OCF3A); // clear any pending interrupts;
152+
ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
153+
#else
154+
TIFR3 = _BV(OCF3A); // clear any pending interrupts;
155+
TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
156+
#endif
157+
#if defined(WIRING)
158+
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
159+
#endif
160+
}
161+
#endif
162+
163+
#if defined (_useTimer4)
164+
if(timer == _timer4) {
165+
TCCR4A = 0; // normal counting mode
166+
TCCR4B = _BV(CS41); // set prescaler of 8
167+
TCNT4 = 0; // clear the timer count
168+
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
169+
TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
170+
}
171+
#endif
172+
173+
#if defined (_useTimer5)
174+
if(timer == _timer5) {
175+
TCCR5A = 0; // normal counting mode
176+
TCCR5B = _BV(CS51); // set prescaler of 8
177+
TCNT5 = 0; // clear the timer count
178+
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
179+
TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
180+
}
181+
#endif
182+
}
183+
184+
static void finISR(timer16_Sequence_t timer)
185+
{
186+
//disable use of the given timer
187+
#if defined WIRING // Wiring
188+
if(timer == _timer1) {
189+
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
190+
TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
191+
#else
192+
TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
193+
#endif
194+
timerDetach(TIMER1OUTCOMPAREA_INT);
195+
}
196+
else if(timer == _timer3) {
197+
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
198+
TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
199+
#else
200+
ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
201+
#endif
202+
timerDetach(TIMER3OUTCOMPAREA_INT);
203+
}
204+
#else
205+
//For arduino - in future: call here to a currently undefined function to reset the timer
206+
#endif
207+
}
208+
209+
static boolean isTimerActive(timer16_Sequence_t timer)
210+
{
211+
// returns true if any servo is active on this timer
212+
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
213+
if(SERVO(timer,channel).Pin.isActive == true)
214+
return true;
215+
}
216+
return false;
217+
}
218+
219+
220+
/****************** end of static functions ******************************/
221+
222+
Servo::Servo()
223+
{
224+
if( ServoCount < MAX_SERVOS) {
225+
this->servoIndex = ServoCount++; // assign a servo index to this instance
226+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
227+
}
228+
else
229+
this->servoIndex = INVALID_SERVO ; // too many servos
230+
}
231+
232+
uint8_t Servo::attach(int pin)
233+
{
234+
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
235+
}
236+
237+
uint8_t Servo::attach(int pin, int min, int max)
238+
{
239+
if(this->servoIndex < MAX_SERVOS ) {
240+
pinMode( pin, OUTPUT) ; // set servo pin to output
241+
servos[this->servoIndex].Pin.nbr = pin;
242+
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
243+
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
244+
this->max = (MAX_PULSE_WIDTH - max)/4;
245+
// initialize the timer if it has not already been initialized
246+
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
247+
if(isTimerActive(timer) == false)
248+
initISR(timer);
249+
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
250+
}
251+
return this->servoIndex ;
252+
}
253+
254+
void Servo::detach()
255+
{
256+
servos[this->servoIndex].Pin.isActive = false;
257+
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
258+
if(isTimerActive(timer) == false) {
259+
finISR(timer);
260+
}
261+
}
262+
263+
void Servo::write(int value)
264+
{
265+
if(value < MIN_PULSE_WIDTH)
266+
{ // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
267+
if(value < 0) value = 0;
268+
if(value > 180) value = 180;
269+
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
270+
}
271+
this->writeMicroseconds(value);
272+
}
273+
274+
void Servo::writeMicroseconds(int value)
275+
{
276+
// calculate and store the values for the given channel
277+
byte channel = this->servoIndex;
278+
if( (channel < MAX_SERVOS) ) // ensure channel is valid
279+
{
280+
if( value < SERVO_MIN() ) // ensure pulse width is valid
281+
value = SERVO_MIN();
282+
else if( value > SERVO_MAX() )
283+
value = SERVO_MAX();
284+
285+
value = value - TRIM_DURATION;
286+
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
287+
288+
uint8_t oldSREG = SREG;
289+
cli();
290+
servos[channel].ticks = value;
291+
SREG = oldSREG;
292+
}
293+
}
294+
295+
int Servo::read() // return the value as degrees
296+
{
297+
return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
298+
}
299+
300+
int Servo::readMicroseconds()
301+
{
302+
unsigned int pulsewidth;
303+
if( this->servoIndex != INVALID_SERVO )
304+
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
305+
else
306+
pulsewidth = 0;
307+
308+
return pulsewidth;
309+
}
310+
311+
bool Servo::attached()
312+
{
313+
return servos[this->servoIndex].Pin.isActive ;
314+
}
315+
316+
#endif // ARDUINO_ARCH_AVR
317+

0 commit comments

Comments
 (0)