Line data Source code
1 : #include "AP_Gripper.h"
2 :
3 : #if AP_GRIPPER_ENABLED
4 :
5 : #include "AP_Gripper_Servo.h"
6 : #include "AP_Gripper_EPM.h"
7 :
8 : extern const AP_HAL::HAL& hal;
9 :
10 : #define GRIPPER_GRAB_PWM_DEFAULT 1900
11 : #define GRIPPER_RELEASE_PWM_DEFAULT 1100
12 : // EPM PWM definitions
13 : #define GRIPPER_NEUTRAL_PWM_DEFAULT 1500
14 : #define GRIPPER_REGRAB_DEFAULT 0 // default EPM re-grab interval (in seconds) to ensure cargo is securely held
15 : #define GRIPPER_AUTOCLOSE_DEFAULT 0.0 // default automatic close time (in seconds)
16 :
17 : const AP_Param::GroupInfo AP_Gripper::var_info[] = {
18 : // @Param: ENABLE
19 : // @DisplayName: Gripper Enable/Disable
20 : // @Description: Gripper enable/disable
21 : // @User: Standard
22 : // @Values: 0:Disabled, 1:Enabled
23 : AP_GROUPINFO_FLAGS("ENABLE", 0, AP_Gripper, _enabled, 0, AP_PARAM_FLAG_ENABLE),
24 :
25 : // @Param: TYPE
26 : // @DisplayName: Gripper Type
27 : // @Description: Gripper enable/disable
28 : // @User: Standard
29 : // @Values: 0:None,1:Servo,2:EPM
30 : AP_GROUPINFO("TYPE", 1, AP_Gripper, config.type, 0),
31 :
32 : // @Param: GRAB
33 : // @DisplayName: Gripper Grab PWM
34 : // @Description: PWM value in microseconds sent to Gripper to initiate grabbing the cargo
35 : // @User: Advanced
36 : // @Range: 1000 2000
37 : // @Units: PWM
38 : AP_GROUPINFO("GRAB", 2, AP_Gripper, config.grab_pwm, GRIPPER_GRAB_PWM_DEFAULT),
39 :
40 : // @Param: RELEASE
41 : // @DisplayName: Gripper Release PWM
42 : // @Description: PWM value in microseconds sent to Gripper to release the cargo
43 : // @User: Advanced
44 : // @Range: 1000 2000
45 : // @Units: PWM
46 : AP_GROUPINFO("RELEASE", 3, AP_Gripper, config.release_pwm, GRIPPER_RELEASE_PWM_DEFAULT),
47 :
48 : // @Param: NEUTRAL
49 : // @DisplayName: Neutral PWM
50 : // @Description: PWM value in microseconds sent to grabber when not grabbing or releasing
51 : // @User: Advanced
52 : // @Range: 1000 2000
53 : // @Units: PWM
54 : AP_GROUPINFO("NEUTRAL", 4, AP_Gripper, config.neutral_pwm, GRIPPER_NEUTRAL_PWM_DEFAULT),
55 :
56 : // @Param: REGRAB
57 : // @DisplayName: EPM Gripper Regrab interval
58 : // @Description: Time in seconds that EPM gripper will regrab the cargo to ensure grip has not weakened; 0 to disable
59 : // @User: Advanced
60 : // @Range: 0 255
61 : // @Units: s
62 : AP_GROUPINFO("REGRAB", 5, AP_Gripper, config.regrab_interval, GRIPPER_REGRAB_DEFAULT),
63 :
64 : // @Param: CAN_ID
65 : // @DisplayName: EPM UAVCAN Hardpoint ID
66 : // @Description: Refer to https://docs.zubax.com/opengrab_epm_v3#UAVCAN_interface
67 : // @User: Standard
68 : // @Range: 0 255
69 : AP_GROUPINFO("CAN_ID", 6, AP_Gripper, config.uavcan_hardpoint_id, 0),
70 :
71 : // @Param: AUTOCLOSE
72 : // @DisplayName: Gripper Autoclose time
73 : // @Description: Time in seconds that gripper close the gripper after opening; 0 to disable
74 : // @User: Advanced
75 : // @Range: 0.25 255
76 : // @Units: s
77 : AP_GROUPINFO("AUTOCLOSE", 7, AP_Gripper, config.autoclose_time, GRIPPER_AUTOCLOSE_DEFAULT),
78 :
79 : AP_GROUPEND
80 : };
81 :
82 2098 : AP_Gripper::AP_Gripper()
83 : {
84 2098 : if (_singleton) {
85 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
86 0 : AP_HAL::panic("Too many grippers");
87 : #endif
88 0 : return;
89 : }
90 2098 : _singleton = this;
91 :
92 2098 : AP_Param::setup_object_defaults(this, var_info);
93 : }
94 :
95 : /*
96 : * Get the AP_Gripper singleton
97 : */
98 : AP_Gripper *AP_Gripper::_singleton = nullptr;
99 50160 : AP_Gripper *AP_Gripper::get_singleton()
100 : {
101 50160 : return _singleton;
102 : }
103 :
104 2094 : void AP_Gripper::init()
105 : {
106 : // return immediately if not enabled
107 2094 : if (!_enabled.get()) {
108 2086 : return;
109 : }
110 :
111 8 : switch(config.type.get()) {
112 0 : case 0:
113 0 : break;
114 : #if AP_GRIPPER_SERVO_ENABLED
115 6 : case 1:
116 6 : backend = NEW_NOTHROW AP_Gripper_Servo(config);
117 6 : break;
118 : #endif
119 : #if AP_GRIPPER_EPM_ENABLED
120 2 : case 2:
121 2 : backend = NEW_NOTHROW AP_Gripper_EPM(config);
122 2 : break;
123 : #endif
124 0 : default:
125 0 : break;
126 : }
127 8 : if (backend != nullptr) {
128 8 : backend->init();
129 : }
130 : }
131 :
132 : // update - should be called at at least 10hz
133 : #define PASS_TO_BACKEND(function_name) \
134 : void AP_Gripper::function_name() \
135 : { \
136 : if (!enabled()) { \
137 : return; \
138 : } \
139 : if (backend != nullptr) { \
140 : backend->function_name(); \
141 : } \
142 : }
143 :
144 24 : PASS_TO_BACKEND(grab)
145 25 : PASS_TO_BACKEND(release)
146 1504089 : PASS_TO_BACKEND(update)
147 :
148 : #undef PASS_TO_BACKEND
149 :
150 :
151 : #define PASS_TO_BACKEND(function_name) \
152 : bool AP_Gripper::function_name() const \
153 : { \
154 : if (!enabled()) { \
155 : return false; \
156 : } \
157 : if (backend != nullptr) { \
158 : return backend->function_name(); \
159 : } \
160 : return false; \
161 : }
162 :
163 24004 : PASS_TO_BACKEND(valid)
164 24003 : PASS_TO_BACKEND(released)
165 0 : PASS_TO_BACKEND(grabbed)
166 :
167 : #undef PASS_TO_BACKEND
168 :
169 : namespace AP {
170 :
171 50160 : AP_Gripper &gripper()
172 : {
173 50160 : return *AP_Gripper::get_singleton();
174 : }
175 :
176 : };
177 :
178 : #endif // AP_GRIPPER_ENABLED
|
