servo.h
Go to the documentation of this file.
1 /*
2  Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
3 
4  This program is free software: you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation, either version 3 of the License, or
7  (at your option) any later version.
8 
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 /*
19  * servo.h
20  *
21  * Created on: 2009-apr-25
22  * Author: Benjamin
23  *
24  *
25  * Changelog:
26  * 2013-12-XX
27  * - Port to ChibiOS and STM32F4
28  * - probably more changes
29  *
30  * 2011-12-08
31  * - Renamed functions
32  * - Fixed pulse issue when pulses are close together
33  *
34  * 2011-07-17
35  * - Changed some variables to volatile.
36  * - Added another command.
37  *
38  * 2010-11-13
39  * - Calculate the servo timings independent of F_CPU.
40  * - Cleanup
41  *
42  */
43 
44 #ifndef SERVO_H_
45 #define SERVO_H_
46 
47 #include "ch.h"
48 #include "hal.h"
49 #include "hw.h"
50 #include "conf_general.h"
51 #include "stm32f4xx_conf.h"
52 
53 #ifndef _BV
54 #define _BV(bit) (1 << (bit))
55 #endif
56 
57 #ifndef MIN
58 #define MIN(a,b) (((a)<(b))?(a):(b))
59 #endif
60 
61 #ifndef MAX
62 #define MAX(a,b) (((a)>(b))?(a):(b))
63 #endif
64 
65 // Change these parameters
66 #define SERVOS_NUM HW_SERVO_NUM // Number of servos to use
67 
68 // Servo timer speed in HZ
69 #define SERVO_CNT_SPEED 1000000L
70 
71 /*
72  * Servo timing (in uS)
73  *
74  * The default signal to drive servos looks like this:
75  *
76  * ----1000us----|----1000us----|---20000us----
77  * _____________________________
78  * |______________|_____...______
79  *
80  * -S_STARTPULSE-|--S_PULSELEN--|--S_COOLDOWN--
81  *
82  * And the default parameters are the following
83  * #define S_STARTPULSE 1000L
84  * #define S_PULSELEN 1000L
85  * #define S_COOLDOWN 20000L
86  *
87  * You can experiment with these to make your servo move further.
88  * For some servos these can be way out of spec.
89  *
90  * Note that S_PULSELEN is not accurate at all for low F_CPU. However,
91  * it will be rounded up to the nearest possible value (hence the strange
92  * calculation below)
93  *
94  */
95 #define S_STARTPULSE SERVO_OUT_PULSE_MIN_US
96 #define S_PULSELEN (SERVO_OUT_PULSE_MAX_US - SERVO_OUT_PULSE_MIN_US)
97 #define S_COOLDOWN (1000000 / SERVO_OUT_RATE_HZ)
98 
99 /*
100  * Dynamic servo parameters
101  * Calculated from F_CPU
102  */
103 #define SERVO_START_OFFSET (SERVO_CNT_SPEED / (1000000L / S_STARTPULSE))
104 #define SERVO_CPU_FACTOR ((SERVO_CNT_SPEED + ((1000000L / S_PULSELEN) * 256L) - 1L) / ((1000000L / S_PULSELEN) * 256L)) // Round up
105 #define SERVO_COOLDOWN_FACTOR (SERVO_CNT_SPEED / (1000000L / S_COOLDOWN))
106 
107 /*
108  * Compile with commands to mode servos with a specified speed
109  * to s specified position interrupt driven. Enabling this will
110  * use some extra ram and a few bytes of flash.
111  */
112 #define USE_COMMANDS 0
113 
114 /*
115  * Calculate how many clock cycles it takes to generate PWM.
116  */
117 #define TEST_CYCLE_TIME 0
118 
119 #if TEST_CYCLE_TIME
120 extern volatile unsigned int restart_cnt;
121 extern volatile unsigned int interrupt_cnt;
122 
123 #define get_restart_cycles() (restart_cnt)
124 #define get_interrupt_cycles() (interrupt_cnt)
125 #endif
126 
127 // Servo macros
128 #define ACTUAL_POS(servo) (MAX(MIN(servo.pos + (signed short)servo.offset, 255), 0))
129 #define ACTUAL_POS_PTR(servo) (MAX(MIN(servo->pos + (signed short)servo->offset, 255), 0))
130 #define CMD_MS_TO_VAL(ms) ((ms) / (((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR) / 1000))
131 #define SERVO_PERIOD_TIME_MS ((S_STARTPULSE + S_PULSELEN + S_COOLDOWN) / 1000)
132 
133 // Some servo speed defines
134 // TODO
135 
136 typedef struct {
137  volatile signed short pos;
138  volatile unsigned char offset;
139  GPIO_TypeDef* gpio;
140  volatile unsigned int pin;
141 } SERVO;
142 
143 #if USE_COMMANDS
144 typedef struct {
145  volatile signed char active;
146  volatile signed short pos;
147  volatile signed short speed;
148  volatile signed short last;
149 } SERVO_CMD;
150 
151 extern volatile signed char cmd_seq_running;
152 extern volatile unsigned int cmd_ptr;
153 extern volatile const signed short *cmd_seq;
154 
155 /*
156  * The number of servo cycles to wait for each time unit in the wait command.
157  *
158  * The wait time can be calculated with:
159  * (S_STARTPULSE + S_PULSELEN + S_COOLDOWN) * CMD_WAIT_FACTOR
160  *
161  */
162 #define CMD_WAIT_FACTOR 1
163 
164 /*
165  * Servo commands.
166  */
167 
168 /*
169  * Move servo to given position with given speed.
170  *
171  * Param 1: Servo.
172  * Param 2: Position.
173  * Param 3: Speed. 0 for max speed.
174  */
175 #define CMD_MOVE_SERVO 0
176 
177 /*
178  * Move servo to given relative position with given speed.
179  *
180  * Param 1: Servo.
181  * Param 2: Relative position.
182  * Param 3: Speed. 0 for max speed.
183  */
184 #define CMD_MOVE_SERVO_REL 1
185 
186 /*
187  * Move multiple servos such that they arrive at the same time
188  *
189  * Param 1: Number of servos.
190  * Param 2: Time for movement in milliseconds
191  * Param 3: Servo 1 index
192  * Param 4: Servo 1 pos
193  * Param 5: Servo 2 index
194  * Param 6: Servo 2 pos
195  * ... and so on
196  */
197 #define CMD_MOVE_MULTIPLE_SERVOS 2
198 
199 /*
200  * Move all servos to center position
201  *
202  * Param 1: Time for movement in milliseconds
203  */
204 #define CMD_CENTER_ALL 3
205 
206 /*
207  * Wait for a while. See configuration for more info.
208  *
209  * Param 1: Time to wait
210  */
211 #define CMD_WAIT 4
212 
213 /*
214  * Wait for servo to be ready.
215  *
216  * Param 1: Servo to wait for.
217  */
218 #define CMD_WAIT_SERVO 5
219 
220 /*
221  * Wait for all servo commands to get ready.
222  */
223 #define CMD_WAIT_ALL_SERVOS 6
224 
225 /*
226  * Stop servo driver.
227  */
228 #define CMD_STOP_DRIVER 7
229 
230 /*
231  * End of command.
232  */
233 #define CMD_STOP_CMDS 8
234 
235 /*
236  * Restart this command sequence
237  */
238 #define CMD_RESTART 9
239 
240 /*
241  * Repeat command sequence a number of times
242  *
243  * Param 1: Number of times to repeat commands.
244  */
245 #define CMD_REPEAT 10
246 #endif
247 
248 extern volatile SERVO servos[SERVOS_NUM];
249 
250 void servo_init(void);
251 void servo_stop_driver(void);
252 unsigned char servo_driver_is_active(void);
253 void servo_irq(void);
254 
255 #if USE_COMMANDS
256 void servo_move(unsigned char servo, signed short position, unsigned char speed);
257 void servo_run_cmds(const signed short *cmds);
258 void servo_stop_cmds(void);
259 void servo_reset_pos(unsigned char speed);
260 void servo_wait_for_cmds(void);
261 void servo_move_within_time(unsigned char servo, signed short pos, unsigned short time_ms);
262 void servo_move_within_time_multiple(unsigned short time_ms, unsigned short num, ...);
263 #endif
264 
265 #endif /* SERVO_H_ */
volatile signed short pos
Definition: servo.h:137
void servo_init(void)
Definition: servo.h:136
unsigned char servo_driver_is_active(void)
volatile unsigned int pin
Definition: servo.h:140
volatile SERVO servos[SERVOS_NUM]
#define SERVOS_NUM
Definition: servo.h:66
void servo_irq(void)
volatile unsigned char offset
Definition: servo.h:138
GPIO_TypeDef * gpio
Definition: servo.h:139
Library configuration file.
void servo_stop_driver(void)