1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
|
/*
Servo.cpp - Interrupt driven Servo library for MSP430
Copyright (c) 2012 Petr Baudis. All right reserved.
Modified by Peter Brier 26-6-2012: Fixed timing/IRQ problem
Derived from:
Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
Copyright (c) 2009 Michael Margolis. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "Energia.h"
#include "Servo.h"
#define F_TIMER (F_CPU/8L)
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to timer ticks (assumes prescale of 8)
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
/************ static functions and data structures common to all instances ***********************/
static servo_t servos[MAX_SERVOS]; // static array of servo structures
static unsigned int ServoCount = 0; // the total number of attached servos
static volatile int counter = 0; // Servo counter; -1 before first servo starts being serviced
static volatile unsigned int totalWait = 0; // Total amount waited so far in the current period; after all servos, wait for the rest of REFRESH_INTERVAL
#ifndef TIMERA0_VECTOR
#define TIMERA0_VECTOR TIMER0_A0_VECTOR
#endif /* TIMER0_A0_VECTOR */
// Timer A0 interrupt service routine
static void
Timer_A(void)
{
static unsigned long wait;
if (counter >= 0) {
/* Turn pulse off. */
digitalWrite(servos[counter].Pin.nbr, LOW);
}
/* Service next servo, while skipping any inactive servo records. */
do {
counter++;
} while (!servos[counter].Pin.isActive && counter < ServoCount);
// Counter range is 0-ServoCount, the last count is used to complete the REFRESH_INTERVAL
if (counter < ServoCount) {
/* Turn pulse on for the next servo. */
digitalWrite(servos[counter].Pin.nbr, HIGH);
/* And hold! */
totalWait += servos[counter].ticks;
CCR0 = servos[counter].ticks;
} else {
/* Wait for the remaining of REFRESH_INTERVAL. */
wait = usToTicks(REFRESH_INTERVAL) - totalWait;
totalWait = 0;
CCR0 = (wait < 1000 ? 1000 : wait);
counter = -1;
}
}
// Timer A0 interrupt service routine
__attribute__((interrupt(TIMERA0_VECTOR)))
static void
Timer_A_int(void)
{
Timer_A();
}
static boolean isTimerActive(void)
{
// returns true if any servo is active
for(int i = 0; i < MAX_SERVOS; i++)
if (servos[i].Pin.isActive == true)
return true;
return false;
}
static void enableTimer(void)
{
counter = -1;
totalWait = 0;
Timer_A(); // enable first servo
CCTL0 = CCIE; // CCR0 interrupt enabled
TACTL = TASSEL_2 + MC_1 + ID_3; // prescale SMCLK/8, upmode
}
static void disableTimer(void)
{
// disable interrupt
CCTL0 = 0;
CCR0 = 0;
}
/****************** end of static functions ******************************/
Servo::Servo()
{
if( ServoCount < MAX_SERVOS) {
this->servoIndex = ServoCount++; // assign a servo index to this instance
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
}
else
this->servoIndex = INVALID_SERVO ; // too many servos
}
uint8_t Servo::attach(int pin)
{
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}
uint8_t Servo::attach(int pin, int min, int max)
{
if(this->servoIndex < MAX_SERVOS ) {
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max)/4;
boolean timer_active = isTimerActive();
servos[this->servoIndex].Pin.isActive = true;
if (!timer_active)
enableTimer();
}
return this->servoIndex ;
}
void Servo::detach()
{
servos[this->servoIndex].Pin.isActive = false;
if (!isTimerActive())
disableTimer();
}
void Servo::write(int value)
{
if(value < MIN_PULSE_WIDTH)
{ // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
if(value < 0) value = 0;
if(value > 180) value = 180;
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
}
void Servo::writeMicroseconds(int value)
{
// calculate and store the values for the given channel
byte channel = this->servoIndex;
if( (channel < MAX_SERVOS) ) // ensure channel is valid
{
if( value < SERVO_MIN() ) // ensure pulse width is valid
value = SERVO_MIN();
else if( value > SERVO_MAX() )
value = SERVO_MAX();
value = value - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
servos[channel].ticks = value; // this is atomic on a 16bit uC, no need to disable Interrupts
}
}
int Servo::read() // return the value as degrees
{
return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
}
int Servo::readMicroseconds()
{
unsigned int pulsewidth;
if( this->servoIndex != INVALID_SERVO )
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
else
pulsewidth = 0;
return pulsewidth;
}
bool Servo::attached()
{
return servos[this->servoIndex].Pin.isActive ;
}
|
