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
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
|
#ifndef __CPHIDGETADVANCEDSERVO
#define __CPHIDGETADVANCEDSERVO
#include "cphidget.h"
/** \defgroup phidadvservo Phidget Advanced Servo
* \ingroup phidgets
* Calls specific to the Phidget Advanced Servo. See the product manual for more specific API details, supported functionality, units, etc.
* @{
*/
DPHANDLE(AdvancedServo)
CHDRSTANDARD(AdvancedServo)
/**
* The Phidget Servo Type sets the relationship of degrees to PCM width
*/
typedef enum {
PHIDGET_SERVO_DEFAULT = 1, /**< Default - This is what the servo API been historically used, originally based on the Futaba FP-S148 */
PHIDGET_SERVO_RAW_us_MODE, /**< Raw us mode - all position, velocity, acceleration functions are specified in microseconds rather then degrees */
PHIDGET_SERVO_HITEC_HS322HD, /**< HiTec HS-322HD Standard Servo */
PHIDGET_SERVO_HITEC_HS5245MG, /**< HiTec HS-5245MG Digital Mini Servo */
PHIDGET_SERVO_HITEC_805BB, /**< HiTec HS-805BB Mega Quarter Scale Servo */
PHIDGET_SERVO_HITEC_HS422, /**< HiTec HS-422 Standard Servo */
PHIDGET_SERVO_TOWERPRO_MG90, /**< Tower Pro MG90 Micro Servo */
PHIDGET_SERVO_HITEC_HSR1425CR, /**< HiTec HSR-1425CR Continuous Rotation Servo */
PHIDGET_SERVO_HITEC_HS785HB, /**< HiTec HS-785HB Sail Winch Servo */
PHIDGET_SERVO_HITEC_HS485HB, /**< HiTec HS-485HB Deluxe Servo */
PHIDGET_SERVO_HITEC_HS645MG, /**< HiTec HS-645MG Ultra Torque Servo */
PHIDGET_SERVO_HITEC_815BB, /**< HiTec HS-815BB Mega Sail Servo */
PHIDGET_SERVO_FIRGELLI_L12_30_50_06_R, /**< Firgelli L12 Linear Actuator 30mm 50:1*/
PHIDGET_SERVO_FIRGELLI_L12_50_100_06_R, /**< Firgelli L12 Linear Actuator 50mm 100:1*/
PHIDGET_SERVO_FIRGELLI_L12_50_210_06_R, /**< Firgelli L12 Linear Actuator 50mm 210:1*/
PHIDGET_SERVO_FIRGELLI_L12_100_50_06_R, /**< Firgelli L12 Linear Actuator 100mm 50:1*/
PHIDGET_SERVO_FIRGELLI_L12_100_100_06_R,/**< Firgelli L12 Linear Actuator 100mm 100:1*/
PHIDGET_SERVO_SPRINGRC_SM_S2313M, /**< SpringRC SM-S2313M Micro Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S3317M, /**< SpringRC SM-S3317M Small Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S3317SR, /**< SpringRC SM-S3317SR Small Continuous Rotation Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S4303R, /**< SpringRC SM-S4303R Standard Continuous Rotation Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S4315M, /**< SpringRC SM-S4315M High Torque Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S4315R, /**< SpringRC SM-S4315R High Torque Continuous Rotation Servo*/
PHIDGET_SERVO_SPRINGRC_SM_S4505B, /**< SpringRC SM-S4505B Standard Servo*/
PHIDGET_SERVO_USER_DEFINED
} CPhidget_ServoType;
/**
* Gets the number of motors supported by this controller
* @param phid An attached phidget advanced servo handle.
* @param count The motor count.
*/
CHDRGET(AdvancedServo,MotorCount,int *count)
/**
* Gets the last set acceleration for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param acceleration The acceleration
*/
CHDRGETINDEX(AdvancedServo,Acceleration,double *acceleration)
/**
* Sets the acceleration for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param acceleration The acceleration
*/
CHDRSETINDEX(AdvancedServo,Acceleration,double acceleration)
/**
* Gets the maximum acceleration supported by a motor
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param max The maximum acceleration.
*/
CHDRGETINDEX(AdvancedServo,AccelerationMax,double *max)
/**
* Gets the minimum acceleration supported by a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param min The minimum acceleration
*/
CHDRGETINDEX(AdvancedServo,AccelerationMin,double *min)
//set velocity
/**
* Gets the last set velocity limit for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param limit The velocity limit.
*/
CHDRGETINDEX(AdvancedServo,VelocityLimit,double *limit)
/**
* Sets the velocity limit for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param limit The velocity limit.
*/
CHDRSETINDEX(AdvancedServo,VelocityLimit,double limit)
//Actual velocity
/**
* Gets the current velocity of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param velocity The current velocity.
*/
CHDRGETINDEX(AdvancedServo,Velocity,double *velocity)
/**
* Gets the maximum velocity that can be set for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param max The maximum velocity
*/
CHDRGETINDEX(AdvancedServo,VelocityMax,double *max)
/**
* Gets the minimum velocity that can be set for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param min The minimum velocity.
*/
CHDRGETINDEX(AdvancedServo,VelocityMin,double *min)
/**
* Sets a velocity change event handler. This is called when the velocity changes.
* @param phid An attached phidget advanced servo handle
* @param fptr Callback function pointer.
* @param userPtr A pointer for use by the user - this value is passed back into the callback function.
*/
CHDREVENTINDEX(AdvancedServo,VelocityChange,double velocity)
/**
* Gets the current position of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param position The position.
*/
CHDRGETINDEX(AdvancedServo,Position,double *position)
/**
* Sets the position of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param position The position.
*/
CHDRSETINDEX(AdvancedServo,Position,double position)
/**
* Gets the maximum position that a motor can go to.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param max The maximum position.
*/
CHDRGETINDEX(AdvancedServo,PositionMax,double *max)
/**
* Sets the maximum position that a motor can go to.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param max The Maximum position.
*/
CHDRSETINDEX(AdvancedServo,PositionMax,double max)
/**
* Gets the minimum position that a motor can go to.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param min The minimum position
*/
CHDRGETINDEX(AdvancedServo,PositionMin,double *min)
/**
* Sets the minimum position that a motor can go to.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param min The minimum position
*/
CHDRSETINDEX(AdvancedServo,PositionMin,double min)
/**
* Sets a position change event handler. This is called when the position changes.
* @param phid An attached phidget advanced servo handle
* @param fptr Callback function pointer.
* @param userPtr A pointer for use by the user - this value is passed back into the callback function.
*/
CHDREVENTINDEX(AdvancedServo,PositionChange,double position)
/**
* Gets the current current draw for a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param current The current.
*/
CHDRGETINDEX(AdvancedServo,Current,double *current)
/**
* Sets a current change event handler. This is called when the current draw changes.
* @param phid An attached phidget advanced servo handle
* @param fptr Callback function pointer.
* @param userPtr A pointer for use by the user - this value is passed back into the callback function.
*/
CHDREVENTINDEX(AdvancedServo,CurrentChange,double current)
/**
* Gets the speed ramping state for a motor. This is whether or not velocity and acceleration are used.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param rampingState The speed ramping state. Possible values are \ref PTRUE and \ref PFALSE.
*/
CHDRGETINDEX(AdvancedServo,SpeedRampingOn,int *rampingState)
/**
* Sets the speed ramping state for a motor. This is whether or not velocity and acceleration are used.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param rampingState The speed ramping state. Possible values are \ref PTRUE and \ref PFALSE.
*/
CHDRSETINDEX(AdvancedServo,SpeedRampingOn,int rampingState)
/**
* Gets the engaged state of a motor. This is whether the motor is powered or not.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param engagedState The engaged state. Possible values are \ref PTRUE and \ref PFALSE.
*/
CHDRGETINDEX(AdvancedServo,Engaged,int *engagedState)
/**
* Sets the engaged state of a motor. This is whether the motor is powered or not.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param engagedState The engaged state. Possible values are \ref PTRUE and \ref PFALSE.
*/
CHDRSETINDEX(AdvancedServo,Engaged,int engagedState)
/**
* Gets the stopped state of a motor. This is true when the motor is not moving and there are no outstanding commands.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param stoppedState The stopped state. Possible values are \ref PTRUE and \ref PFALSE.
*/
CHDRGETINDEX(AdvancedServo,Stopped,int *stoppedState)
/**
* Gets the servo type of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param servoType The servo type.
*/
CHDRGETINDEX(AdvancedServo,ServoType,CPhidget_ServoType *servoType)
/**
* Sets the servo type of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param servoType The servo type.
*/
CHDRSETINDEX(AdvancedServo,ServoType,CPhidget_ServoType servoType)
/**
* Sets the servo parameters of a motor.
* @param phid An attached phidget advanced servo handle
* @param index The motor index.
* @param min_us The minimum supported PCM in microseconds.
* @param max_us The maximum supported PCM in microseconds.
* @param degrees The degrees of rotation defined by the given PCM range.
* @param velocity_max The maximum velocity in degrees/second.
*/
CHDRSETINDEX(AdvancedServo,ServoParameters,double min_us,double max_us,double degrees,double velocity_max)
#ifndef EXTERNALPROTO
#define ADVSERVO_MAXSERVOS 8
#define ADVSERVO_NORMAL_PACKET 0x00
#define ADVSERVO_MINMAX_PACKET 0x10
//Flags - need to match those defined in firmware
#define MOTOR_DONE_ADVSERVO 0x20
#define MOTOR_DISABLED_ADVSERVO 0x40
#define NO_RAMPING_FLAG_ADVSERVO 0x80
#define ADVSERVO_PGOOD_FLAG 0x01
//Define the parameters for several servo motors
struct _CPhidgetServoParameters
{
CPhidget_ServoType servoType;
double min_us;
double max_us;
double us_per_degree;
double max_us_per_s; //velocity in us notation
unsigned char state;
} typedef CPhidgetServoParameters, *CPhidgetServoParametersHandle;
extern const CPhidgetServoParameters Phid_Servo_Types[];
int getServoParameterIndex(CPhidget_ServoType type);
PHIDGET21_API double CCONV servo_us_to_degrees(CPhidgetServoParameters params, double us, unsigned char round);
PHIDGET21_API double CCONV servo_degrees_to_us(CPhidgetServoParameters params, double degrees);
PHIDGET21_API double CCONV servo_us_to_degrees_vel(CPhidgetServoParameters params, double us, unsigned char round);
PHIDGET21_API double CCONV servo_degrees_to_us_vel(CPhidgetServoParameters params, double degrees);
PHIDGET21_API int CCONV setupNewAdvancedServoParams(CPhidgetAdvancedServoHandle phid, int Index, CPhidgetServoParameters newVal);
struct _CPhidgetAdvancedServo {
CPhidget phid;
int (CCONV *fptrPositionChange)(CPhidgetAdvancedServoHandle, void *, int, double);
int (CCONV *fptrVelocityChange)(CPhidgetAdvancedServoHandle, void *, int, double);
int (CCONV *fptrCurrentChange)(CPhidgetAdvancedServoHandle, void *, int, double);
void *fptrPositionChangeptr;
void *fptrVelocityChangeptr;
void *fptrCurrentChangeptr;
//all data is stored as microseconds
//data from the device
double motorVelocityEcho[ADVSERVO_MAXSERVOS];
double motorPositionEcho[ADVSERVO_MAXSERVOS];
double motorSensedCurrent[ADVSERVO_MAXSERVOS];
unsigned char motorSpeedRampingStateEcho[ADVSERVO_MAXSERVOS];
unsigned char motorEngagedStateEcho[ADVSERVO_MAXSERVOS];
unsigned char motorStoppedState[ADVSERVO_MAXSERVOS];
int packetCounterEcho[ADVSERVO_MAXSERVOS];
//data from the user
double motorPosition[ADVSERVO_MAXSERVOS];
double motorAcceleration[ADVSERVO_MAXSERVOS];
double motorVelocity[ADVSERVO_MAXSERVOS];
unsigned char motorSpeedRampingState[ADVSERVO_MAXSERVOS];
unsigned char motorEngagedState[ADVSERVO_MAXSERVOS];
int packetCounter[ADVSERVO_MAXSERVOS];
double motorPositionMax[ADVSERVO_MAXSERVOS];
double motorPositionMin[ADVSERVO_MAXSERVOS];
double velocityMax[ADVSERVO_MAXSERVOS], velocityMin;
double velocityMaxLimit;
double accelerationMax, accelerationMin;
double motorPositionMaxLimit, motorPositionMinLimit;
unsigned char PGoodErrState;
CPhidgetServoParameters servoParams[ADVSERVO_MAXSERVOS];
char *servoParamString[ADVSERVO_MAXSERVOS];
unsigned char outputPacket[8];
unsigned int outputPacketLen;
} typedef CPhidgetAdvancedServoInfo;
#endif
/** @} */
#endif
|