All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
smartmotor.h
Go to the documentation of this file.
1 /*-----------------------------------------------------------------------------*/
2 /* */
3 /* Copyright (c) James Pearman */
4 /* 2013 */
5 /* All Rights Reserved */
6 /* */
7 /*-----------------------------------------------------------------------------*/
8 /* */
9 /* Module: SmartMotorLib.h */
10 /* Author: James Pearman */
11 /* Created: 2 Oct 2012 */
12 /* V1.10 4 July 2013 - Initial release fopr ChibiOS */
13 /* */
14 /*-----------------------------------------------------------------------------*/
15 /* */
16 /* This file is part of ConVEX. */
17 /* */
18 /* The author is supplying this software for use with the VEX cortex */
19 /* control system. ConVEX is free software; you can redistribute it */
20 /* and/or modify it under the terms of the GNU General Public License */
21 /* as published by the Free Software Foundation; either version 3 of */
22 /* the License, or (at your option) any later version. */
23 /* */
24 /* ConVEX is distributed in the hope that it will be useful, */
25 /* but WITHOUT ANY WARRANTY; without even the implied warranty of */
26 /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
27 /* GNU General Public License for more details. */
28 /* */
29 /* You should have received a copy of the GNU General Public License */
30 /* along with this program. If not, see <http://www.gnu.org/licenses/>. */
31 /* */
32 /* A special exception to the GPL can be applied should you wish to */
33 /* distribute a combined work that includes ConVEX, without being obliged */
34 /* to provide the source code for any proprietary components. */
35 /* See the file exception.txt for full details of how and when the */
36 /* exception can be applied. */
37 /* */
38 /* The author can be contacted on the vex forums as jpearman */
39 /* or electronic mail using jbpearman_at_mac_dot_com */
40 /* Mentor for team 8888 RoboLancers, Pasadena CA. */
41 /* */
42 /*-----------------------------------------------------------------------------*/
43 
44 #ifndef __SMARTMOTORLIB__
45 #define __SMARTMOTORLIB__
46 
47 /*-----------------------------------------------------------------------------*/
48 /** @file smartmotor.h
49  * @brief A port of the smart motor library for ChibiOS/RT, header
50 *//*---------------------------------------------------------------------------*/
51 
52 #include "ch.h" // needs for all ChibiOS programs
53 #include "hal.h" // hardware abstraction layer header
54 #include "vex.h"
55 
56 // Version 1.11
57 #define kSmartMotorLibVersion 111
58 
59 // System parameters - don't change
60 #define SMLIB_R_SYS 0.3
61 #define SMLIB_PWM_FREQ 1150
62 #define SMLIB_V_DIODE 0.75
63 
64 // parameters for vex 393 motor
65 #define SMLIB_I_FREE_393 0.2
66 #define SMLIB_I_STALL_393 4.8
67 #define SMLIB_RPM_FREE_393 110
68 #define SMLIB_R_393 (7.2/SMLIB_I_STALL_393)
69 #define SMLIB_L_393 0.000650
70 #define SMLIB_Ke_393 (7.2*(1-SMLIB_I_FREE_393/SMLIB_I_STALL_393)/SMLIB_RPM_FREE_393)
71 #define SMLIB_I_SAFE393 0.90
72 
73 // parameters for vex 269 motor
74 #define SMLIB_I_FREE_269 0.18
75 #define SMLIB_I_STALL_269 2.88
76 #define SMLIB_RPM_FREE_269 120
77 #define SMLIB_R_269 (7.2/SMLIB_I_STALL_269)
78 #define SMLIB_L_269 0.000650
79 #define SMLIB_Ke_269 (7.2*(1-SMLIB_I_FREE_269/SMLIB_I_STALL_269)/SMLIB_RPM_FREE_269)
80 #define SMLIB_I_SAFE269 0.75
81 
82 // parameters for cortex and Power expander
83 // spec says 4A but we have set a little lower here
84 // may increase in a subsequent release.
85 #define SMLIB_I_SAFECORTEX 3.0
86 #define SMLIB_I_SAFEPE 3.0
87 
88 // encoder counts per revolution depending on motor
89 #define SMLIB_TPR_269 240.448
90 #define SMLIB_TPR_393S 392
91 #define SMLIB_TPR_393T 627.2
92 #define SMLIB_TPR_QUAD 360.0
93 #define SMLIB_TPR_POT 6000.0 // estimate
94 
95 // Initial ambient temp 72deg F in deg C
96 #define SMLIB_TEMP_AMBIENT (( 72.0-32.0) * 5 / 9)
97 // Trip temperature for PTC 100 deg C, may be a little low
98 #define SMLIB_TEMP_TRIP 100.0
99 // Trip hysteresis in deg C, once tripped we need a 10 deg drop to enable normal operation
100 #define SMLIB_TEMP_HYST 10.0
101 // Reference temperature for data below, 25 deg C
102 #define SMLIB_TEMP_REF 25.0
103 
104 // Hold current is the current where thr PTC should not trip
105 // Time to trip is the time at 5 x hold current
106 // K_TAU is a safety factor, probably 0.5 ~ 0.8
107 // we are being conservative here, it trip occurs too soon then increase
108 //
109 // PTC HR16-400 used in cortex and power expander
110 #define SMLIB_I_HOLD_CORTEX 3.0
111 #define SMLIB_T_TRIP_CORTEX 1.7
112 #define SMLIB_K_TAU_CORTEX 0.5
113 #define SMLIB_TAU_CORTEX (SMLIB_K_TAU_CORTEX * SMLIB_T_TRIP_CORTEX * 5.0 * 5.0)
114 #define SMLIB_C1_CORTEX ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_CORTEX * SMLIB_I_HOLD_CORTEX) )
115 #define SMLIB_C2_CORTEX (1.0 / (SMLIB_TAU_CORTEX * 1000.0))
116 
117 // PTC HR30-090 used in 393
118 //#define SMLIB_I_HOLD_393 0.9
119 #define SMLIB_I_HOLD_393 1.0 // increased a little to slow down trip
120 #define SMLIB_T_TRIP_393 7.1
121 #define SMLIB_K_TAU_393 0.5
122 #define SMLIB_TAU_393 (SMLIB_K_TAU_393 * SMLIB_T_TRIP_393 * 5.0 * 5.0)
123 #define SMLIB_C1_393 ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_393 * SMLIB_I_HOLD_393) )
124 #define SMLIB_C2_393 (1.0 / (SMLIB_TAU_393 * 1000.0))
125 
126 // PTC HR16-075 used in 269
127 #define SMLIB_I_HOLD_269 0.75
128 #define SMLIB_T_TRIP_269 2.0
129 #define SMLIB_K_TAU_269 0.5
130 #define SMLIB_TAU_269 (SMLIB_K_TAU_269 * SMLIB_T_TRIP_269 * 5.0 * 5.0)
131 #define SMLIB_C1_269 ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_269 * SMLIB_I_HOLD_269) )
132 #define SMLIB_C2_269 (1.0 / (SMLIB_TAU_269 * 1000.0))
133 
134 // No way to test 3 wire - Vamfun had them more or less the same as the 269
135 // PTC MINISMDC-075F used in three wire
136 #define SMLIB_C1_3WIRE SMLIB_C1_269
137 #define SMLIB_C2_3WIRE SMLIB_C2_269
138 
139 #define SMLIB_LEDON 0
140 #define SMLIB_LEDOFF 1
141 
142 
143 /*-----------------------------------------------------------------------------*/
144 /* */
145 /* This large structure holds all the needed information for a single motor */
146 /* */
147 /* The constants used in calculations are all set automatically by the init */
148 /* code. Other variables are used by the different functions to calculate */
149 /* speed, current and temperature. some of these are stored for debug */
150 /* purposes */
151 /* */
152 /* V1.02 code uses 124 bytes (a couple are wasted due to word alignment) */
153 /* so 1240 bytes in all for the 10 motors. */
154 /*-----------------------------------------------------------------------------*/
155 
156 typedef struct {
157  // the motor port
158  tVexMotor port;
159 
160  // the motor encoder port
161  tVexMotor eport;
162 
163  // copy of the system motor type
164  tVexMotorType type;
165 
166  // due to alignment we get a free variable here
167  // use for debugging loop delay
168  short delayTimeMs;
169 
170  // pointer to our control bank
171  struct _smartController *bank;
172 
173  // commanded speed comes from the user
174  // requested speed is either the commanded speed or limited speed if the PTC
175  // is about to trip
176  short motor_cmd;
177  short motor_req;
178  short motor_slew;
179 
180  // current limit and max cmd value
181  short limit_tripped;
182  short limit_cmd;
183  float limit_current;
184 
185  // the encoder associated with this motor
186  short encoder_id;
187  // encoder ticks per rev
188  float ticks_per_rev;
189 
190  // variables used by rpm calculation
191  long enc;
192  long oldenc;
193  float delta;
194  float rpm;
195 
196  // variables used for current calculation
197  float i_free;
198  float i_stall;
199  float r_motor;
200  float l_motor;
201  float ke_motor;
202  float rpm_free;
203  float v_bemf_max;
204 
205  // instantaneous current
206  float current;
207  // a filtered version of current to remove some transients
208  float filtered_current;
209  // peak measured current
210  float peak_current;
211 
212  // holds safe current for this motor
213  float safe_current;
214  // target current in limited mode
215  float target_current;
216 
217  // PTC monitor variables
218  float temperature;
219  float t_const_1;
220  float t_const_2;
221  float t_ambient;
222  short ptc_tripped;
223 
224  // Last program time we ran - may not keep this, bit overkill
225  long lastPgmTime;
226  } smartMotor;
227 
228 /*-----------------------------------------------------------------------------*/
229 /* Motor control related definitions */
230 /*-----------------------------------------------------------------------------*/
231 
232 #define SMLIB_MOTOR_MAX_CMD 127 // maximum command value to motor
233 #define SMLIB_MOTOR_MIN_CMD (-127) // minimum command value to motor
234 #define SMLIB_MOTOR_DEFAULT_SLEW_RATE 10 // Default will cause 375mS from full fwd to rev
235 #define SMLIB_MOTOR_FAST_SLEW_RATE 256 // essentially off
236 #define SMLIB_MOTOR_DEADBAND 10 // values below this are set to 0
237 
238 // When current limit is not needed set limit_cmd to this value
239 #define SMLIB_MOTOR_MAX_CMD_UNDEFINED 255 // special value for limit_motor
240 
241 /*-----------------------------------------------------------------------------*/
242 /* */
243 /* This structure holds all the information for a controller bank */
244 /* which is a PTC protected circuit in the cortex or power expander */
245 /* */
246 /* We limit storage to three banks as that is the limit for a VEX competition */
247 /* robot although you could have more power expanders in theory */
248 /* */
249 /* V1.00 code uses 52 bytes per bank */
250 /*-----------------------------------------------------------------------------*/
251 
252 // 3 banks of maximum 5 motors (even though power expander is 4)
253 #define SMLIB_TOTAL_NUM_CONTROL_BANKS 3
254 #define SMLIB_TOTAL_NUM_BANK_MOTORS 5
255 // Index for banks
256 #define SMLIB_CORTEX_PORT_0 0
257 #define SMLIB_CORTEX_PORT_1 1
258 #define SMLIB_PWREXP_PORT_0 2
259 
260 typedef struct _smartController {
261  // array of pointers to the motors
262  smartMotor *motors[SMLIB_TOTAL_NUM_BANK_MOTORS];
263 
264  // cumulative current
265  float current;
266  // peak measured current
267  float peak_current;
268  // holds safe current for this motor
269  float safe_current;
270 
271  // PTC monitor variables
272  float temperature;
273  float t_const_1;
274  float t_const_2;
275  float t_ambient;
276 
277  // flag for ptc status
278  short ptc_tripped;
279 
280  // Do we have an led to show tripped status
281  tVexDigitalPin statusLed;
282 
283  // Power expander may have a status port
284  tVexAnalogPin statusPort;
285  } smartController;
286 
287 
288 // We have no inline so use a macro as shortcut to get ptr
289 #define _SmartMotorGetPtr( index ) ((smartMotor *)&sMotors[ index ])
290 // We have no inline so use a macro as shortcut to get ptr
291 #define _SmartMotorControllerGetPtr( index ) ((smartController *)&sPorts[ index ])
292 
293 // Initialization
294 void SmartMotorsInit( void );
295 void SmartMotorLinkMotors( tVexMotor master, tVexMotor slave );
296 void SmartMotorsSetEncoderGearing( tVexMotor index, float ratio );
297 #define SmartMotorsAddPowerExtender( p0, ... ) \
298  _SmartMotorsAddPowerExtender( p0, ##__VA_ARGS__, -1, -1, -1 )
299 void _SmartMotorsAddPowerExtender( int p0, int p1, int p2, int p3, ... );
300 
301 // Status
302 float SmartMotorGetSpeed( tVexMotor index );
303 #define SmartMotorGetCurrent(index, ... ) \
304  _SmartMotorGetCurrent( index, ##__VA_ARGS__, 0 )
305 float _SmartMotorGetCurrent( tVexMotor index, int s, ... );
306 float SmartMotorGetTemperature( tVexMotor index );
307 int SmartMotorGetLimitCmd( tVexMotor index );
308 
309 float SmartMotorGetControllerCurrent( short index );
310 float SmartMotorGetControllerTemperature( short index );
311 
312 // Control
313 void SmartMotorPtcMonitorEnable( void );
314 void SmartMotorPtcMonitorDisable( void );
315 void SmartMotorCurrentMonitorEnable( void );
316 void SmartMotorCurrentMonitorDisable( void );
317 #define SmartMotorSetLimitCurent(index, ... ) \
318  _SmartMotorSetLimitCurent( index, ##__VA_ARGS__, 1.0 )
319 void _SmartMotorSetLimitCurent( tVexMotor index, float current, ... );
320 void SmartMotorSetFreeRpm( tVexMotor index, short max_rpm );
321 void SmartMotorSetSlewRate( tVexMotor index, int slew_rate );
322 void SmartMotorRun( void );
323 void SmartMotorStop( void );
324 void SmartMotorSetControllerStatusLed( int index, tVexDigitalPin port );
325 void SmartMotorSetPowerExpanderStatusPort( tVexAnalogPin port );
326 void SmartMotorDebugStatus(void);
327 #define SmartMotorSetRpmSensor( index, port, ticks_per_rev, ... ) \
328  _SmartMotorSetRpmSensor( index, port, ticks_per_rev, ##__VA_ARGS__, FALSE )
329 void _SmartMotorSetRpmSensor( tVexMotor index, tVexAnalogPin port, float ticks_per_rev, bool_t revesed, ... );
330 
331 #define SetMotor( index, value, ... ) \
332  _SetMotor( index, value, ##__VA_ARGS__, FALSE )
333 void _SetMotor( int index, int value, bool_t immediate, ... );
334 
335 // Access raw data
336 smartMotor *SmartMotorGetPtr( tVexMotor index );
337 smartController *SmartMotorControllerGetPtr( short index );
338 
339 // Private functions for reference
340 void SmartMotorSpeed( smartMotor *m, int deltaTime );
341 void SmartMotorSimulateSpeed( smartMotor *m );
342 float SmartMotorCurrent( smartMotor *m, float v_battery );
343 float SmartMotorControllerCurrent( smartController *s );
344 int SmartMotorSafeCommand( smartMotor *m, float v_battery );
345 float SmartMotorTemperature( smartMotor *m, int deltaTime );
346 float SmartMotorControllerTemperature( smartController *s, int deltaTime );
347 void SmartMotorMonitorPtc( smartMotor *m, float v_battery );
348 void SmartMotorControllerMonitorPtc( smartController *s, float v_battery );
349 void SmartMotorMonitorCurrent( smartMotor *m, float v_battery );
350 void SmartMotorControllerSetLed( smartController *s );
351 msg_t SmartMotorTask( void *arg );
352 msg_t SmartMotorSlewRateTask( void *arg );
353 
354 #endif // __SMARTMOTORLIB__