servo360 library  v0.9.2
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 
995 int servo360_enable(int pin, int state);
996 
997 
1008 int servo360_feedback(int pin, int state);
1009 
1010 
1023 int servo360_set(int pinControl, int time);
1024 
1025 
1047 int servo360_setAngleCalc(int pin, int angle);
1048 
1049 
1065 int servo360_getAngleCalc(int pin);
1066 
1067 
1074 #ifndef DOXYGEN_SHOULD_SKIP_THIS
1075 
1076 
1077 
1078 /* Private */
1079 void servo360_run(void);
1080 void servo360_end();
1081 void servo360_setup(void);
1082 void servo360_mainLoop();
1083 void servo360_outputSelector(int p);
1084 
1085 void servo360_servoPulse(int p, int q);
1086 void servo360_waitServoCtrllEdgeNeg(int p);
1087 int servo360_dutyCycle(int p, int scale);
1088 int servo360_crossing(int current, int previous, int units);
1089 int servo360_getTheta(int p);
1090 void servo360_checkAngle(int p);
1091 void servo360_captureOffset(int p);
1092 void servo360_setPositiveDirection(int p, int direction);
1093 
1094 int servo360_setRampStep(int p, int stepSize);
1095 
1096 int servo360_upsToPulseFromTransferFunction(int unitsPerSec, int p);
1097 void servo360_pulseControl(int p, int speedUps);
1098 
1099 void servo360_speedControl(int p);
1100 int servo360_pidA(int p);
1101 int servo360_pidV(int p);
1102 
1103 int servo360_findServoIndex(int pin);
1104 
1105 int servo360_unitsAngleToEncoder(int value, int unitsRev);
1106 int servo360_unitsEncoderToAngle(int value, int unitsRev);
1107 int servo360_checkDistanceRemaining(int pin, int speed, int finalAngle);
1108 
1109 int servo360_setMaxSpeedEncoded(int pin, int speed);
1110 
1111 void servo360_monitorRun(void);
1112 void servo360_monitorEnd(void);
1113 
1114 int servo360_setTransferFunction(int pin, int constant, int value);
1115 
1116 
1117 int servo360_getAngle12Bit(int pin);
1118 int servo360_getAngleFixedOrigin(int pin);
1119 
1120 
1121 /*
1122 __attribute__((constructor))
1123 void servo360_patch(void);
1124 */
1125 
1126 #ifdef _servo360_monitor_
1127 void console();
1128 void servo360_monitor_start(void);
1129 void servo360_monitor_stop();
1130 int terminal_checkForValue(fdserial *connection, int *value);
1131 #endif
1132 
1133 
1134 
1135 #endif // DOXYGEN_SHOULD_SKIP_THIS
1136 
1137 
1138 
1139 #if defined(__cplusplus)
1140 }
1141 #endif
1142 /* __cplusplus */
1143 #endif
1144 /* SERVO360_H */
1145 
1146 
1169  //
servo360_getAngleCalc
int servo360_getAngleCalc(int pin)
During servo360_speed maneuvers, the target position gets recalculated every 50th of a second...
Definition: servo360_getAngleCalc.c:17
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 actu...
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_setAngleCalc
int servo360_setAngleCalc(int pin, int angle)
During servo360_speed maneuvers, the target position gets recalculated every 50th of a second...
Definition: servo360_setAngleCalc.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 imp...
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:16
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
Generated on Fri Oct 19 2018 13:36:34 for servo360 library by   doxygen 1.8.14