Wixel SDK
servo.c
1 #include <servo.h>
2 
25 #define MAX_SERVOS 6
26 
27 // Keeps track of whether the library has been enabled or not.
28 static BIT servosStartedFlag = 0;
29 
30 // Keeps track of whether there are servos moving or not (position != target).
31 // This is updated in the ISR.
32 static volatile BIT servosMovingFlag = 0;
33 
34 volatile uint8 DATA servoCounter = 0;
35 
36 // Associates external channel number (the number picked by the user) to the
37 // internal channel number.
38 static uint8 XDATA servoAssignment[MAX_SERVOS];
39 
42 struct SERVO_DATA
43 {
48 };
49 
50 static volatile struct SERVO_DATA XDATA servoData[MAX_SERVOS];
51 
52 // Bitmasks for keeping track of which pins are being used as servos.
53 // A 1 bit indicates that the pin is a servo pulse output pin.
54 // A 0 bit indicates that the pin will be used for something else and
55 // this library should not touch it.
56 static volatile uint8 servoPinsOnPort0;
57 static volatile uint8 servoPinsOnPort1;
58 
59 ISR(T1, 0)
60 {
61  uint8 i;
62 
63  switch(servoCounter++)
64  {
65  case 0:
66  PERCFG &= ~(1<<6); // PERCFG.T1CFG = 0: Move Timer 1 to Alt. 1 location (P0_2, P0_3, P0_4)
67  P0SEL |= servoPinsOnPort0;
68  T1CC0 = servoData[0].positionReg; // NOTE: T1CCx is buffered, so these commands
69  T1CC1 = servoData[1].positionReg; // don't take effect until the next timer period.
70  T1CC2 = servoData[2].positionReg;
71  break;
72 
73  case 3:
74  PERCFG |= (1<<6); // PERCFG.T1CFG = 1: Move Timer 1 to Alt. 2 location (P1_2, P1_1, P1_0)
75  P1SEL |= servoPinsOnPort1;
76  T1CC0 = servoData[3].positionReg;
77  T1CC1 = servoData[4].positionReg;
78  T1CC2 = servoData[5].positionReg;
79  break;
80 
81  case 1:
82  case 4:
83  // We are producing pulses during THIS period, so disable the pulses for the next timer period.
84  T1CC0 = T1CC1 = T1CC2 = 0xFFFF;
85  break;
86 
87  case 2:
88  // The pulses on port 0 just finished, so assign the pins to be GPIO (driving low) again.
89  P0SEL &= ~servoPinsOnPort0;
90  break;
91 
92  case 5:
93  // The pulses on port 1 just finished, so assign the pins to be GPIO (driving low) again.
94  P1SEL &= ~servoPinsOnPort1;
95  break;
96 
97  case 6:
98  // Set the counter back to zero so that next time we will start over at the beginning.
99  servoCounter = 0;
100 
101  // Update the positions of all the servos according to their speed limits,
102  // and update servosMovingFlag.
103 
104  // David measured how long these updates take, and it is only about 70us even if there is
105  // a speed limit enabled for all channels.
106  // WARNING: The SDCC manual warns that 16-bit division, multiplication, and modulus are implemented
107  // using external support routines that are not reentrant, so we can't do any of those operations here!
108  // The assembly generated by this ISR in servo.lst should be checked whenever making changes to the ISR.
109 
110  servosMovingFlag = 0;
111 
112  for(i = 0; i < MAX_SERVOS; i++)
113  {
114  volatile struct SERVO_DATA XDATA * d = servoData + i;
115  uint16 pos = d->position;
116 
117  if (d->speed && pos)
118  {
119  if (d->target > pos)
120  {
121  if (d->target - pos < d->speed)
122  {
123  pos = d->target;
124  }
125  else
126  {
127  pos += d->speed;
128  servosMovingFlag = 1;
129  }
130  }
131  else
132  {
133  if (pos - d->target < d->speed)
134  {
135  pos = d->target;
136  }
137  else
138  {
139  pos -= d->speed;
140  servosMovingFlag = 1;
141  }
142  }
143  }
144  else
145  {
146  pos = d->target;
147  }
148  d->position = pos;
149  d->positionReg = ~pos + 1;
150  }
151 
152  break;
153  }
154 }
155 
156 static uint8 pinToInternalChannelNumber(uint8 pin)
157 {
158  switch(pin)
159  {
160  case 2: return 0;
161  case 3: return 1;
162  case 4: return 2;
163  case 12: return 3;
164  case 11: return 4;
165  case 10: return 5;
166  default: return 0;
167  }
168 }
169 
170 void servosStart(uint8 XDATA * pins, uint8 numPins)
171 {
172  uint8 i;
173 
174  if (servosStartedFlag)
175  {
176  servosStop();
177  }
178 
180 
181  // The user passes a null argument for pins, then don't reinitialize the pins.
182  // This allows us to temporarily start and stop the servos without losing track
183  // of the old speeds, targets, and positions.
184  if (pins != 0)
185  {
186  servoPinsOnPort0 = servoPinsOnPort1 = 0;
187  for (i = 0; i < MAX_SERVOS; i++)
188  {
189  servoData[i].target = 0;
190  servoData[i].position = 0;
191  servoData[i].positionReg = 0;
192  servoData[i].speed = 0;
193 
194  if (i < numPins)
195  {
196  uint8 internalChannelNumber = pinToInternalChannelNumber(pins[i]);
197 
198  servoAssignment[i] = internalChannelNumber;
199 
200  switch(internalChannelNumber)
201  {
202  case 0: P0_2 = 0; servoPinsOnPort0 |= (1<<2); break;
203  case 1: P0_3 = 0; servoPinsOnPort0 |= (1<<3); break;
204  case 2: P0_4 = 0; servoPinsOnPort0 |= (1<<4); break;
205  case 3: P1_2 = 0; servoPinsOnPort1 |= (1<<2); break;
206  case 4: P1_1 = 0; servoPinsOnPort1 |= (1<<1); break;
207  case 5: P1_0 = 0; servoPinsOnPort1 |= (1<<0); break;
208  }
209  }
210  }
211 
212  // Set all the pins being used to be general-purpose outputs driving low for now.
213  P0SEL &= ~servoPinsOnPort0;
214  P0DIR |= servoPinsOnPort0;
215  P1SEL &= ~servoPinsOnPort1;
216  P1DIR |= servoPinsOnPort1;
217 
218  if (servoPinsOnPort0)
219  {
220  // Set PRIP0[1:0] to 11 (Timer 1 channel 2 - USART0).
221  // I'm not sure why this is necessary, but if it is not set then
222  // Timer 1 can not control P0_4, even if no other peripherals on Port 0 are enabled.
223  P2DIR |= 0b11000000;
224  }
225  }
226 
228 
229  // Turn off the timer and reset the counters.
230  T1CTL = 0;
231  T1CNTL = 0; // resets high and low bytes
232  servoCounter = 0;
233 
234  // Configure Timer 1 Channels 0-2 to be in compare mode. Set output on compare-up, clear on 0.
235  // This means all three pulses will start at different times but end at the same time.
236  // With this configuration, we can set T1CC0, T1CC1, or T1CC2 to -N to get a pulse of with N,
237  // as long as N > 1.
238  // We can set the register to -1 or 0 to disable the pulse.
239  T1CCTL0 = T1CCTL1 = T1CCTL2 = 0b00011100;
240 
241  // Turn off all the pulses at first.
242  T1CC0 = T1CC1 = T1CC2 = 0xFFFF;
243 
244  // Timer 1: Start free-running mode, counting from 0x0000 to 0xFFFF.
245  T1CTL = 0b00000001;
246 
247  // Set the Timer 1 interrupt priority to 2, the second highest.
248  IP0 &= ~(1<<1);
249  IP1 |= (1<<1);
250  T1IE = 1; // Enable the Timer 1 interrupt.
251  EA = 1; // Enable interrupts in general.
252 
253  servosStartedFlag = 1;
254 }
255 
256 void servosStop(void)
257 {
258  if (!servosStartedFlag)
259  {
260  // The servos have already been stopped.
261  return;
262  }
263 
264  T1IE = 0;
265 
266  // Wait for the timer to overflow.
267  while(!T1IF){};
268 
269  // Assuming that there were fewer than (2730 - MAX_SERVO_TARGET_MICROSECONDS) worth of
270  // interrupts the time when T1IF was read as true and now, the timer has just
271  // overflowed and the next servo pulses have not started yet.
272  // Make the pins revert to GPIO outputs driving low:
273  P0SEL &= ~servoPinsOnPort0;
274  P1SEL &= ~servoPinsOnPort1;
275 
276  // Turn off Timer 1.
277  T1CTL = 0;
278 
279  servosStartedFlag = 0;
280 }
281 
283 {
284  return servosStartedFlag;
285 }
286 
288 {
289  return servosMovingFlag;
290 }
291 
292 void servoSetTarget(uint8 servoNum, uint16 targetMicroseconds)
293 {
294  // Convert the units of target from microseconds to timer ticks.
295  servoSetTargetHighRes(servoNum, targetMicroseconds * SERVO_TICKS_PER_MICROSECOND);
296 }
297 
299 {
300  volatile struct SERVO_DATA XDATA * d = servoData + servoAssignment[servoNum];
301 
302  // TODO: return here if "target" is out of the valid range
303 
304  T1IE = 0; // Make sure we don't get interrupted in the middle of an update.
305 
306  // Make this function have an immediate effect, if necessary.
307  if (d->speed == 0 || d->target == 0 || target == 0)
308  {
309  d->position = target;
310  d->positionReg = ~target + 1;
311  }
312  else if (target != d->position)
313  {
314  servosMovingFlag = 1;
315  }
316 
317  d->target = target;
318 
319  T1IE = servosStartedFlag;
320 }
321 
323 {
324  return servoData[servoAssignment[servoNum]].target / SERVO_TICKS_PER_MICROSECOND;
325 }
326 
328 {
330 }
331 
333 {
334  return servoData[servoAssignment[servoNum]].target;
335 }
336 
338 {
340  T1IE = 0; // Make sure we don't get interrupted in the middle of reading the position.
341  position = servoData[servoAssignment[servoNum]].position;
342  T1IE = servosStartedFlag;
343  return position;
344 }
345 
347 {
348  T1IE = 0; // Make sure we don't get interrupted in the middle of an update.
349  servoData[servoAssignment[servoNum]].speed = speed;
350  T1IE = servosStartedFlag;
351 }
352 
354 {
355  return servoData[servoAssignment[servoNum]].speed;
356 }
#define DATA
Definition: cc2511_types.h:52
void servosStart(uint8 XDATA *pins, uint8 numPins)
Definition: servo.c:170
void servoSetSpeed(uint8 servoNum, uint16 speed)
Definition: servo.c:346
void servoSetTarget(uint8 servoNum, uint16 targetMicroseconds)
Definition: servo.c:292
void servosStop(void)
Definition: servo.c:256
#define XDATA
Definition: cc2511_types.h:65
uint16 servoGetSpeed(uint8 servoNum)
Definition: servo.c:353
void servoSetTargetHighRes(uint8 servoNum, uint16 target)
Definition: servo.c:298
uint16 position
Definition: servo.c:45
uint16 servoGetTargetHighRes(uint8 servoNum)
Definition: servo.c:332
uint16 servoGetPosition(uint8 servoNum)
Definition: servo.c:327
__bit BIT
Definition: cc2511_types.h:32
#define ISR(source, bank)
Definition: cc2511_map.h:71
BIT servosMoving(void)
Definition: servo.c:287
unsigned short uint16
Definition: cc2511_types.h:15
unsigned char uint8
Definition: cc2511_types.h:9
uint16 speed
Definition: servo.c:47
uint16 target
Definition: servo.c:44
uint16 servoGetTarget(uint8 servoNum)
Definition: servo.c:322
uint16 positionReg
Definition: servo.c:46
uint16 servoGetPositionHighRes(uint8 servoNum)
Definition: servo.c:337
BIT servosStarted(void)
Definition: servo.c:282
#define SERVO_TICKS_PER_MICROSECOND
Definition: servo.h:86