servo360 library v0.9.1 Parallax Feedback 360 High Speed Servo functions for control, monitoring, and setting

servo360.h
Go to the documentation of this file.
1
19 //#define _servo360_monitor_
20
21
22
23 #ifndef SERVO360_H
24 #define SERVO360_H
25
26 #if defined(__cplusplus)
27 extern "C" {
28 #endif
29
30
31 #include "simpletools.h"
32 //#include "patch.h"
33
34 #ifdef _servo360_monitor_
35 #include "fdserial.h"
36 #endif
37
38
39
40 #ifndef DOXYGEN_SHOULD_SKIP_THIS
41
42
43
44 #ifndef S360_UNITS_FULL_CIRCLE
45 #define S360_UNITS_FULL_CIRCLE 360
46 #endif
47
48
49 #ifndef S360_MAX_SPEED
50 #define S360_MAX_SPEED 2 * 4096
51 #endif
52
53 #ifndef S360_VM
54 #define S360_VM 180
55 #endif
56 //#define S360_VB_POS 200
57 //#define S360_VB_NEG -200
58 #ifndef S360_VM_CCW
59 #define S360_VM_CCW S360_VM
60 #endif
61
62 #ifndef S360_VM_CW
63 #define S360_VM_CW S360_VM
64 #endif
65
66 #ifndef S360_VM_CCW
67 #define S360_VM_CCW S360_VM
68 #endif
69
70 #ifndef S360_VM_CW
71 #define S360_VM_CW S360_VM
72 #endif
73
74 #ifndef S360_VB_CCW
75 #define S360_VB_CCW 200
76 #endif
77
78 #ifndef S360_VB_CW
79 #define S360_VB_CW -200
80 #endif
81
82
83 #ifndef S360_DUTY_CYCLE_MIN
84 #define S360_DUTY_CYCLE_MIN 290
85 #endif
86
87 #ifndef S360_DUTY_CYCLE_MAX
88 #define S360_DUTY_CYCLE_MAX 9710
89 #endif
90
91
92 #ifndef S360_CCW_POS
93 #define S360_CCW_POS 1
94 #endif
95
96 #ifndef S360_CCW_NEG
97 #define S360_CCW_NEG -1
98 #endif
99
100
101 #ifndef S360_SPEED
102 #define S360_SPEED 1
103 #endif
104
105 #ifndef S360_POSITION
106 #define S360_POSITION 2
107 #endif
108
109 #ifndef S360_GOTO
110 #define S360_GOTO 3
111 #endif
112
113 #ifndef S360_MONITOR
114 #define S360_MONITOR 4
115 #endif
116
117 #ifndef S360_FREQ_CTRL_SIG
118 #define S360_FREQ_CTRL_SIG 50
119 #endif
120
121 #ifndef S360_DEVS_MAX
122 #define S360_DEVS_MAX 4
123 #endif
124
125 //#define S360_RAMP_STEP 10 * 4096 / 360;
126
127 #ifndef S360_RAMP_STEP
128 #define S360_RAMP_STEP 72 * 4096 / 360
129 #endif
130
131 // Rename to indicate encoder
132
133 #ifndef S360_M
134 #define S360_M 4348
135 #endif
136
137 #ifndef S360_B
138 #define S360_B -129
139 #endif
140
141 #ifndef S360_ENC_RES
142 #define S360_ENC_RES 4096
143 #endif
144
145 /*
146 #define S360_KPA 5000
147 #define S360_KIA 150
148 #define S360_KDA 0
149 #define S360_POS_INTGRL_MAX 150
150 #define S360_SCALE_DEN_A 1000
151 */
152
153 //
154
155 #ifndef S360_KPA
156 #define S360_KPA 12000
157 #endif
158
159 #ifndef S360_KIA
160 #define S360_KIA 600
161 #endif
162
163 #ifndef S360_KDA
164 #define S360_KDA 6000
165 #endif
166
167 #ifndef S360_POS_INTGRL_MAX
168 #define S360_POS_INTGRL_MAX 600
169 #endif
170
171 #ifndef S360_SCALE_DEN_A
172 #define S360_SCALE_DEN_A 1000
173 #endif
174 //
175
176 /*
177 #define S360_KPV 3000
178 #define S360_KIV 500
179 #define S360_KDV 1600
180 #define S360_VEL_INTGRL_MAX 325
181 */
182
183 #ifndef S360_KPV
184 #define S360_KPV 500
185 #endif
186
187 #ifndef S360_KIV
188 #define S360_KIV 0
189 #endif
190
191 #ifndef S360_KDV
192 #define S360_KDV 0
193 #endif
194
195 #ifndef S360_VEL_INTGRL_MAX
196 #define S360_VEL_INTGRL_MAX 0
197 #endif
198
199 #ifndef S360_SCALE_DEN_V
200 #define S360_SCALE_DEN_V 1000
201 #endif
202
203
204 #ifndef S360_PW_CENTER
205 #define S360_PW_CENTER 15000
206 #endif
207
208 #ifndef S360_CS_HZ
209 #define S360_CS_HZ 50
210 #endif
211
212 #ifndef S360_UNITS_ENCODER
213 #define S360_UNITS_ENCODER 4096
214 #endif
215
216 #ifndef S360_UNITS_REV
217 #define S360_UNITS_REV 360
218 #endif
219
220
221 #ifndef S360_SETTING_KPV
222 #define S360_SETTING_KPV 1
223 #endif
224
225 #ifndef S360_SETTING_KIV
226 #define S360_SETTING_KIV 2
227 #endif
228
229 #ifndef S360_SETTING_KDV
230 #define S360_SETTING_KDV 3
231 #endif
232
233 #ifndef S360_SETTING_IV_MAX
234 #define S360_SETTING_IV_MAX 4
235 #endif
236
237 #ifndef S360_SETTING_KPA
238 #define S360_SETTING_KPA 5
239 #endif
240
241 #ifndef S360_SETTING_KIA
242 #define S360_SETTING_KIA 6
243 #endif
244
245 #ifndef S360_SETTING_KDA
246 #define S360_SETTING_KDA 7
247 #endif
248
249 #ifndef S360_SETTING_IA_MAX
250 #define S360_SETTING_IA_MAX 8
251 #endif
252
253 #ifndef S360_SETTING_VB_CCW
254 #define S360_SETTING_VB_CCW 9
255 #endif
256
257 #ifndef S360_SETTING_VB_CW
258 #define S360_SETTING_VB_CW 10
259 #endif
260
261 #ifndef S360_SETTING_VM_CCW
262 #define S360_SETTING_VM_CCW 11
263 #endif
264
265 #ifndef S360_SETTING_VM_CW
266 #define S360_SETTING_VM_CW 12
267 #endif
268
269 //#define S360_A_MAX (((1 << 31) - 1)) / S360_UNITS_FULL_CIRCLE // 524287 degrees
270
271 #ifndef S360_A_MAX
272 #define S360_A_MAX 524287
273 #endif
274
275
276 #ifndef S360_SCALE_DEN_COUPLE
277 #define S360_SCALE_DEN_COUPLE 1000
278 #endif
279
280 #ifndef S360_SCALE_COUPLE
281 #define S360_SCALE_COUPLE 2000
282 #endif
283
284
285 #ifndef S360_LATENCY
286 #define S360_LATENCY 3
287 #endif
288
289
290 #ifndef S360_PWMAX
291 #define S360_PWMAX 2400
292 #endif
293
294 #ifndef S360_PWMIN
295 #define S360_PWMIN -2400
296 #endif
297
298
299 /*
300
301 // WIP servo calibration updates
302 //40564
303
304 #ifndef _AB360_EE_Start_
305 #define _AB360_EE_Start_ 63418
306 #endif
307
308 #ifndef _AB360_EE_Pins_
309 #define _AB360_EE_Pins_ 12
310 #endif
311
312 #ifndef _AB360_EE_mVccwL_
313 #define _AB360_EE_mVccwL_ 28
314 #endif
315
316 #ifndef _AB360_EE_bVccwL_
317 #define _AB360_EE_bVccwL_ 32
318 #endif
319
320 #ifndef _AB360_EE_mVcwL_
321 #define _AB360_EE_mVcwL_ 36
322 #endif
323
324 #ifndef _AB360_EE_bVcwL_
325 #define _AB360_EE_bVcwL_ 40
326 #endif
327
328 #ifndef _AB360_EE_mVccwR_
329 #define _AB360_EE_mVccwR_ 44
330 #endif
331
332 #ifndef _AB360_EE_bVccwR_
333 #define _AB360_EE_bVccwR_ 48
334 #endif
335
336 #ifndef _AB360_EE_mVcwR_
337 #define _AB360_EE_mVcwR_ 52
338 #endif
339
340 #ifndef _AB360_EE_bVcwR_
341 #define _AB360_EE_bVcwR_ 56
342 #endif
343
344 #ifndef _AB360_EE_End_
345
346 #define _AB360_EE_End_ _AB360_EE_Start_ + 60
347 #endif
348
349 */
350
351
352
354 //
355 typedef volatile struct servo360_cog_s
356 {
357 int *servoCog;
358 volatile int lock360;
359 volatile int t360;
360 volatile int t360slice;
361 volatile int fbSlice;
362 volatile int spSlice;
363 volatile int devCount;
364 volatile int pulseCount;
365 volatile int cntPrev;
366 volatile int dt360;
367 volatile int dt360a[2];
368 } servo360_cog_t;
369
370 extern volatile servo360_cog_t _fb360c;
371 //
373
374 typedef volatile struct servo360_s
375 {
376 // settings
377 volatile int pinCtrl; // pinControl
378 volatile int pinFb; // pinFeedback
379 volatile int angleSign;
380 volatile int rampStep;
381 volatile int speedLimit;
382 volatile int feedback;
383 volatile int angleMax;
384 volatile int angleMin;
385 volatile int unitsRev;
386 volatile int couple;
387 volatile int coupleScale;
388 volatile int enable;
389
390 volatile int vmCcw;
391 volatile int vmCw;
392 volatile int vbCcw;
393 volatile int vbCw;
394
395 // admin
396 volatile int csop;
397 volatile int speedReq;
398
399 // encoders
400 volatile int dc, dcp;
401 volatile int theta, thetaP;
402 volatile int turns;
403 volatile int angleFixed, angleFixedP;
404 volatile int angle, angleP, pvOffset;
405 //volatile int stalled, noSignal;
406 volatile int opMax;
407
408 // pulse control
409 volatile int speedOut;
410 volatile int pw;
411
412 // velocity control system
413 volatile int opPidV;
414 volatile int approachFlag;
415 volatile int ticksDiff;
416 volatile int ticksGuard;
417 volatile int angleCalc;
418 volatile int angleDeltaCalc;
419 volatile int angleError;
420 volatile int erDist, erDistP;
421 volatile int speedTarget;
422 volatile int speedTargetP;
423 volatile int integralV;
424 volatile int derivativeV;
425 volatile int opV;
426 volatile int iMaxV;
427 volatile int iMinV;
428 volatile int KpV;
429 volatile int KiV;
430 volatile int KdV;
431 volatile int pV, iV, dV;
432 volatile int speedMeasured;
433 volatile int drive;
434 volatile int stepDir;
435 volatile int lag;
436 volatile int accelerating;
437 volatile int speedTargetTemp;
438
439 /*
440 // This could remedy the overshoot problem, but it seems to reduce
441 // drive_goto accuracy.
442
443 #define FB360_OFFSET_MAX 5
444 #define FB360_V_ARRAY 8
445
446 volatile int vT[FB360_V_ARRAY];
447 volatile int offset;
448 */
449
450 // position control system
451 volatile int Kp;
452 volatile int Ki;
453 volatile int Kd;
454 volatile int er, erP;
455 volatile int sp, integral, derivative;
456 volatile int op, iMax, iMin;
457 volatile int p, i, d;
458
459 // goto system
460 volatile int angleTarget;
461 }
462 servo360_t;
463
464 extern volatile servo360_t _fs[S360_DEVS_MAX];
465
466 // console
467 #ifdef _servo360_monitor_
468 fdserial *term;
469 extern volatile int suppressFbDisplay;
470 #endif
471
472
473
474 #endif // DOXYGEN_SHOULD_SKIP_THIS
475
476
477
495 int servo360_connect(int pinControl, int pinFeedback);
496
497
498
516 int servo360_angle(int pin, int position);
517
518 int servo360_getAngle(int pin);
526
527
528
548 int servo360_speed(int pin, int speed);
549
550 int servo360_getSpeed(int pin);
559
560
575 int servo360_goto(int pin, int position);
576
577
589 int servo360_getCsop(int pin);
590
591
592
601 int servo360_stop(int pin);
602
603
604
624 int servo360_setAcceleration(int pin, int unitsPerSecSquared);
625
626
635 int servo360_getAcceleration(int pin);
636
637
638
648 int servo360_setMaxSpeed(int pin, int speed);
649
650
658 int servo360_getMaxSpeed(int pin);
659
660
661
673 int servo360_setRampStep(int pin, int stepSize);
674
675
686 int servo360_getRampStep(int pin);
687
688
689
714 int servo360_setAngleLimits(int pin, int ccwMax, int cwMax);
715
716
730 int servo360_getAngleLimits(int pin, int *ccwMax, int *cwMax);
731
732
733
744 int servo360_setAngleCtrlSpeedMax(int pin, int speed);
745
746
755 int servo360_getAngleCtrlSpeedMax(int pin);
756
757
758
770 int servo360_setAngleOffset(int pin, int angle);
771
772
782 int servo360_getAngleOffset(int pin);
783
784
785
816 int servo360_setUnitsFullCircle(int pin, int units);
817
818
829 int servo360_getUnitsFullCircle(int pin);
830
831
832
882 int servo360_setControlSys(int pin, int constant, int value);
883
884
895 int servo360_getControlSys(int pin, int constant);
896
897
898
914 int servo360_setTurns(int pin, int turns);
915
916
927 int servo360_getTurns(int pin);
928
929
930
953 int servo360_couple(int pinA, int pinB);
954
955
974 int servo360_setCoupleScale(int pinA, int pinB, int scale);
975
976
977
998 int servo360_enable(int pin, int state);
999
1000
1011 int servo360_feedback(int pin, int state);
1012
1013
1026 int servo360_set(int pinControl, int time);
1027
1028
1029
1036 #ifndef DOXYGEN_SHOULD_SKIP_THIS
1037
1038
1039
1040 /* Private */
1041 void servo360_run(void);
1042 void servo360_end();
1043 void servo360_setup(void);
1044 void servo360_mainLoop();
1045 void servo360_outputSelector(int p);
1046
1047 void servo360_servoPulse(int p, int q);
1048 void servo360_waitServoCtrllEdgeNeg(int p);
1049 int servo360_dutyCycle(int p, int scale);
1050 int servo360_crossing(int current, int previous, int units);
1051 int servo360_getTheta(int p);
1052 void servo360_checkAngle(int p);
1053 void servo360_captureOffset(int p);
1054 void servo360_setPositiveDirection(int p, int direction);
1055
1056 int servo360_setRampStep(int p, int stepSize);
1057
1058 int servo360_upsToPulseFromTransferFunction(int unitsPerSec, int p);
1059 void servo360_pulseControl(int p, int speedUps);
1060
1061 void servo360_speedControl(int p);
1062 int servo360_pidA(int p);
1063 int servo360_pidV(int p);
1064
1065 int servo360_findServoIndex(int pin);
1066
1067 int servo360_unitsAngleToEncoder(int value, int unitsRev);
1068 int servo360_unitsEncoderToAngle(int value, int unitsRev);
1069 int servo360_checkDistanceRemaining(int pin, int speed, int finalAngle);
1070
1071 int servo360_setMaxSpeedEncoded(int pin, int speed);
1072
1073 void servo360_monitorRun(void);
1074 void servo360_monitorEnd(void);
1075
1076 int servo360_setTransferFunction(int pin, int constant, int value);
1077
1078
1079 int servo360_getAngle12Bit(int pin);
1080 int servo360_getAngleFixedOrigin(int pin);
1081 int servo360_getAngleCalc(int pin);
1082
1083 /*
1084 __attribute__((constructor))
1085 void servo360_patch(void);
1086 */
1087
1088 #ifdef _servo360_monitor_
1089 void console();
1090 void servo360_monitor_start(void);
1091 void servo360_monitor_stop();
1092 int terminal_checkForValue(fdserial *connection, int *value);
1093 #endif
1094
1095
1096
1097 #endif // DOXYGEN_SHOULD_SKIP_THIS
1098
1099
1100
1101 #if defined(__cplusplus)
1102 }
1103 #endif
1104 /* __cplusplus */
1105 #endif
1106 /* SERVO360_H */
1107
1108
1131 //
servo360_setUnitsFullCircle
int servo360_setUnitsFullCircle(int pin, int units)
Set the number of units in a full circle. By default, this value is 360. This function does not actua...
Definition: servo360_setUnitsFullCircle.c:17
servo360_setTurns
int servo360_setTurns(int pin, int turns)
Change the number of turns that have elapsed since the application has started. Use this function to ...
Definition: servo360_setTurns.c:17
servo360_getUnitsFullCircle
int servo360_getUnitsFullCircle(int pin)
Get the number of units in a full circle. By default, this value is 360, but it can be changed with s...
Definition: servo360_getUnitsFullCircle.c:17
servo360_angle
int servo360_angle(int pin, int position)
Set an angle in degrees for servo to move to and hold. The default degree is 1/360th of a full circle...
Definition: servo360_angle.c:17
servo360_enable
int servo360_enable(int pin, int state)
Enable or disable the control system signal.
Definition: servo360_enable.c:17
servo360_getRampStep
int servo360_getRampStep(int pin)
Get current acceleration setting in terms of degrees per second per 50th of a second. For a more straightforward implementation, use servo360_getAcceleration instead.
Definition: servo360_getRampStep.c:17
servo360_connect
int servo360_connect(int pinControl, int pinFeedback)
Initializes a connection to a Parallax Feedback 360 servo. The current position of the servo during t...
Definition: servo360_connect.c:19
servo360_speed
int servo360_speed(int pin, int speed)
Set servo rotation speed in degrees per second. The default max values range from 720 degrees per sec...
Definition: servo360_speed.c:17
servo360_getControlSys
int servo360_getControlSys(int pin, int constant)
Check control system constant. See the servo360_setControlSys function for more info.
Definition: servo360_getControlSys.c:17
servo360_setCoupleScale
int servo360_setCoupleScale(int pinA, int pinB, int scale)
Change the scale factor in a pair of servos that were coupled with the servo360_couple call...
Definition: servo360_setCoupleScale.c:17
servo360_getMaxSpeed
int servo360_getMaxSpeed(int pin)
Get the max speed setting.
Definition: servo360_getMaxSpeed.c:17
servo360_getAngleLimits
int servo360_getAngleLimits(int pin, int *ccwMax, int *cwMax)
Check the angle limits the servo is allowed to turn to during position control. These limits only aff...
Definition: servo360_getAngleLimits.c:17
servo360_getAngleOffset
int servo360_getAngleOffset(int pin)
Check the offset from the servo&#39;s mechanical 0-degree position. If servo360_setAngleOffset has not be...
Definition: servo360_getAngleOffset.c:17
servo360_getTurns
int servo360_getTurns(int pin)
Get the number of times the output shaft has turned in a full circle since the application started (e...
Definition: servo360_getTurns.c:16
servo360_setAngleLimits
int servo360_setAngleLimits(int pin, int ccwMax, int cwMax)
Set the angle limits the servo is allowed to turn to during position control. These limits only affec...
Definition: servo360_setAngleLimits.c:17
servo360_setControlSys
int servo360_setControlSys(int pin, int constant, int value)
The servo360.h library is designed to use Proportional, Integral and Derivative (PID) control to main...
Definition: servo360_setControlSys.c:16
servo360_getAcceleration
int servo360_getAcceleration(int pin)
Get current acceleration in terms of degrees per second squared.
Definition: servo360_getAcceleration.c:17
servo360_setMaxSpeed
int servo360_setMaxSpeed(int pin, int speed)
Set the maximum servo speed in terms of degrees per second.
Definition: servo360_setMaxSpeed.c:17
servo360_feedback
int servo360_feedback(int pin, int state)
Enable or disable the feedback system. If the feedback system is disabled, the servo should only be c...
Definition: servo360_feedback.c:17
servo360_setRampStep
int servo360_setRampStep(int pin, int stepSize)
Set acceleration in terms of degrees per second per 50th of a second. For a more straightforward impl...
Definition: servo360_setRampStep.c:17
servo360_setAcceleration
int servo360_setAcceleration(int pin, int unitsPerSecSquared)
Set acceleration in terms of degrees per second squared. The default is 3600 degrees per second squar...
Definition: servo360_setAcceleration.c:17
servo360_goto
int servo360_goto(int pin, int position)
Go to a certain number of degrees from the current position. Positive values are counterclockwise, negative values are clockwise. This is a "set it and forget it" function that returns immediately after the maneuver has been initiated. The maneuver&#39;s progress can be monitored with servo360_getCsop and also with servo360_getAngle.
Definition: servo360_goto.c:17
servo360_getSpeed
int servo360_getSpeed(int pin)
Measure the current speed. For best results, take the average of multiple measurements.
Definition: servo360_getSpeed.c:17
servo360_setAngleCtrlSpeedMax
int servo360_setAngleCtrlSpeedMax(int pin, int speed)
Set the maximum speed at which the servo is allowed to turn while adjusting to a new angle set point...
Definition: servo360_setAngleCtrlSpeedMax.c:17
servo360_getAngle
int servo360_getAngle(int pin)
Get measured angle of servo in degree units.
Definition: servo360_getAngle.c:17
servo360_getAngleCtrlSpeedMax
int servo360_getAngleCtrlSpeedMax(int pin)
Check the maximum speed at which the servo is allowed to turn while adjusting to a new angle set poin...
Definition: servo360_getAngleCtrlSpeedMax.c:17
servo360_couple
int servo360_couple(int pinA, int pinB)
This function is used by the abdrive360 library to reduce the drive on a servo whose position is furt...
Definition: servo360_couple.c:17
servo360_setAngleOffset
int servo360_setAngleOffset(int pin, int angle)
Set the offset from the servo&#39;s mechanical 0 degree position. If this function is not called...
Definition: servo360_setAngleOffset.c:17
servo360_getCsop
int servo360_getCsop(int pin)
Csop is an abbreviation for control system operation, which could be speed, position, or goto control. During a servo360_goto maneuver, this function will return S360_GOTO. When the maneuver has completed, it will return S360_POSITION.
Definition: servo360_getCsop.c:17
servo360_stop
int servo360_stop(int pin)
Stop servo motion if it is turning. It is equivalent to a call to servo360_speed(pin, 0).
Definition: servo360_stop.c:17
servo360_set
int servo360_set(int pinControl, int time)
Use pulse width control to set servo speed without feedback. Make sure to call serov360_feedback(pin...
Definition: servo360_set.c:17