amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ 9c46b728
History | View | Annotate | Download (10.62 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "ch.hpp" |
---|---|---|---|
2 | #include "hal.h" |
||
3 | #include "qei.h" |
||
4 | #include "DiWheelDrive.h" |
||
5 | |||
6 | 9c46b728 | galberding | |
7 | 58fe0e0b | Thomas Schöpping | #include <global.hpp> |
8 | |||
9 | using namespace chibios_rt; |
||
10 | using namespace amiro; |
||
11 | using namespace types; |
||
12 | |||
13 | extern volatile uint32_t shutdown_now; |
||
14 | extern Global global;
|
||
15 | |||
16 | DiWheelDrive::DiWheelDrive(CANDriver *can) |
||
17 | : ControllerAreaNetworkTx(can, CAN::DI_WHEEL_DRIVE_ID), |
||
18 | ControllerAreaNetworkRx(can, CAN::DI_WHEEL_DRIVE_ID), |
||
19 | bcCounter(0)
|
||
20 | { |
||
21 | } |
||
22 | |||
23 | msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) { |
||
24 | int deviceId = this->decodeDeviceId(frame); |
||
25 | |||
26 | switch (deviceId) {
|
||
27 | |||
28 | case CAN::SHELL_REPLY_ID(CAN::DI_WHEEL_DRIVE_ID):
|
||
29 | if (frame->DLC > 0) { |
||
30 | sdWrite(&SD1, frame->data8, frame->DLC); |
||
31 | return RDY_OK;
|
||
32 | } |
||
33 | break;
|
||
34 | |||
35 | case CAN::SHELL_QUERY_ID(CAN::DI_WHEEL_DRIVE_ID):
|
||
36 | if (frame->DLC != 0) { |
||
37 | global.sercanmux1.convCan2Serial(frame->data8, frame->DLC); |
||
38 | return RDY_OK;
|
||
39 | } else {
|
||
40 | global.sercanmux1.rcvSwitchCmd(this->decodeBoardId(frame));
|
||
41 | return RDY_OK;
|
||
42 | } |
||
43 | break;
|
||
44 | |||
45 | case CAN::TARGET_SPEED_ID:
|
||
46 | if (frame->DLC == 8) { |
||
47 | global.distcontrol.deactivateController(); |
||
48 | kinematic targetVelocity; |
||
49 | targetVelocity.x = frame->data32[0];
|
||
50 | targetVelocity.w_z = frame->data32[1];
|
||
51 | global.motorcontrol.setTargetSpeed(targetVelocity); |
||
52 | return RDY_OK;
|
||
53 | } |
||
54 | break;
|
||
55 | |||
56 | case CAN::TARGET_RPM_ID:
|
||
57 | if (frame->DLC == 8) { |
||
58 | global.distcontrol.deactivateController(); |
||
59 | global.motorcontrol.setTargetRPM(frame->data32[0], frame->data32[1]); |
||
60 | return RDY_OK;
|
||
61 | } |
||
62 | break;
|
||
63 | |||
64 | case CAN::SET_ODOMETRY_ID:
|
||
65 | if (frame->DLC == 8) { |
||
66 | int32_t robotPositionX = (frame->data8[0] << 8 | frame->data8[1] << 16 | frame->data8[2] << 24); |
||
67 | int32_t robotPositionY = (frame->data8[3] << 8 | frame->data8[4] << 16 | frame->data8[5] << 24); |
||
68 | int32_t robotPositionF_Z = (frame->data8[6] << 8 | frame->data8[7] << 16); |
||
69 | global.odometry.setPosition(float(robotPositionX)*1e-6,float(robotPositionY)*1e-6,float(robotPositionF_Z)*1e-6); |
||
70 | return RDY_OK;
|
||
71 | } |
||
72 | break;
|
||
73 | |||
74 | case CAN::BROADCAST_SHUTDOWN:
|
||
75 | if (frame->DLC == 2 && frame->data16[0] == CAN::SHUTDOWN_MAGIC) { |
||
76 | shutdown_now = 0x4;
|
||
77 | return RDY_OK;
|
||
78 | } |
||
79 | break;
|
||
80 | |||
81 | case CAN::CALIBRATE_PROXIMITY_FLOOR:
|
||
82 | // Dont care about the payload but start the calibration
|
||
83 | // TODO Care about the payload. Differ between:
|
||
84 | // 1: Do fresh calibration (Save values to memory and to temporary values)
|
||
85 | // 2: Remove temporary Calibration and get uncalibrated values
|
||
86 | // 3: Load calibration from memory
|
||
87 | this->calibrate();
|
||
88 | break;
|
||
89 | |||
90 | case CAN::TARGET_POSITION_ID:
|
||
91 | if (frame->DLC == 8) { |
||
92 | // Robot target position [x] = µm, [f_z] = µrad, [t] = ms
|
||
93 | int32_t robotPositionX = (frame->data8[0] << 8 | frame->data8[1] << 16 | frame->data8[2] << 24); |
||
94 | int32_t robotPositionF_Z = (frame->data8[3] << 8 | frame->data8[4] << 16 | frame->data8[5] << 24); |
||
95 | uint16_t targetTimeMilliSeconds = (frame->data8[6] | frame->data8[7] << 8); |
||
96 | //chprintf((BaseSequentialStream*) &SD1, "\nx=%d\nf_z=%d\nt=%d", robotPositionX, robotPositionF_Z, targetTimeMilliSeconds);
|
||
97 | global.distcontrol.setTargetPosition(robotPositionX, robotPositionF_Z, targetTimeMilliSeconds); |
||
98 | return RDY_OK;
|
||
99 | } |
||
100 | break;
|
||
101 | c76baf23 | Georg Alberding | case CAN::SET_LINE_FOLLOW_SPEED:
|
102 | 9c46b728 | galberding | if (frame->DLC == 8) { |
103 | uint8_t speedForward = frame->data8[0];
|
||
104 | uint8_t speedSoftLeft0 = frame->data8[1];
|
||
105 | uint8_t speedSoftLeft1 = frame->data8[2];
|
||
106 | uint8_t speedHardLeft0 = frame->data8[3];
|
||
107 | uint8_t speedHardLeft1 = frame->data8[4];
|
||
108 | global.rpmForward[0] = speedForward;
|
||
109 | global.rpmForward[1] = speedForward;
|
||
110 | global.rpmSoftLeft[0] = speedSoftLeft0;
|
||
111 | global.rpmSoftLeft[1] = speedSoftLeft1;
|
||
112 | global.rpmHardLeft[0] = speedHardLeft0;
|
||
113 | global.rpmHardLeft[1] = speedHardLeft1;
|
||
114 | global.rpmSoftRight[0] = global.rpmSoftLeft[1]; |
||
115 | global.rpmSoftRight[1] = global.rpmSoftLeft[0]; |
||
116 | global.rpmHardRight[0] = global.rpmHardLeft[1]; |
||
117 | global.rpmHardRight[1] = global.rpmHardLeft[0]; |
||
118 | return RDY_OK;
|
||
119 | } |
||
120 | break;
|
||
121 | case CAN::SET_LINE_FOLLOW_STRATEGY:
|
||
122 | if (frame->DLC == 1) { |
||
123 | global.lfStrategy = frame->data8[0];
|
||
124 | return RDY_OK;
|
||
125 | } |
||
126 | break;
|
||
127 | 58fe0e0b | Thomas Schöpping | case CAN::SET_KINEMATIC_CONST_ID:
|
128 | if (frame->DLC == 8) { |
||
129 | /* // Set (but do not store) Ed
|
||
130 | global.motorcontrol.setWheelDiameterCorrectionFactor(static_cast<float>(frame->data32[0]), false);
|
||
131 | // Set (but do not store) Eb
|
||
132 | global.motorcontrol.setActualWheelBaseDistance(static_cast<float>(frame->data32[1]), false);
|
||
133 | return RDY_OK;*/
|
||
134 | // Set (but do not store) Ed
|
||
135 | uint32_t ed_int = static_cast<uint32_t>(frame->data32[0]); |
||
136 | float ed_float = static_cast<float>(ed_int)/1000000.0; |
||
137 | global.motorcontrol.setWheelDiameterCorrectionFactor(ed_float, false);
|
||
138 | // Set (but do not store) Eb
|
||
139 | uint32_t eb_int = static_cast<uint32_t>(frame->data32[1]); |
||
140 | float eb_float = static_cast<float>(eb_int)/1000000.0; |
||
141 | global.motorcontrol.setActualWheelBaseDistance(eb_float, false);
|
||
142 | //chprintf((BaseSequentialStream*) &SD1, "Edi=%i, Edf=%f, Ebi=%i, Ebf=%f\n", ed_int, ed_float, eb_int, eb_float);
|
||
143 | return RDY_OK;
|
||
144 | } |
||
145 | break;
|
||
146 | |||
147 | case CAN::POWER_STATUS_ID:
|
||
148 | if (frame->DLC == 6) { |
||
149 | // The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
|
||
150 | types::power_status::ChargingState charging_flags; |
||
151 | charging_flags.value = frame->data8[0];
|
||
152 | global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path); |
||
153 | // Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
|
||
154 | } |
||
155 | break;
|
||
156 | |||
157 | default:
|
||
158 | break;
|
||
159 | } |
||
160 | return -1; |
||
161 | } |
||
162 | |||
163 | msg_t DiWheelDrive::updateSensorVal() { |
||
164 | |||
165 | // Update robot velocity values
|
||
166 | kinematic currentVelocity = global.motorcontrol.getCurrentVelocity(); |
||
167 | this->actualSpeed[0] = currentVelocity.x; |
||
168 | this->actualSpeed[1] = currentVelocity.w_z; |
||
169 | |||
170 | // Update odometry values
|
||
171 | this->robotPosition = global.odometry.getPosition();
|
||
172 | |||
173 | // Update proximity values
|
||
174 | for (int idx = 0; idx < 4; ++idx) |
||
175 | this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
|
||
176 | |||
177 | b4885314 | Thomas Schöpping | // Update magnetometer values
|
178 | for (uint8_t axis = 0; axis < 3; ++axis) { |
||
179 | this->magnetometerValue[axis] = global.hmc5883l.getMagnetizationGauss(axis);
|
||
180 | } |
||
181 | |||
182 | // Update gyroscope values
|
||
183 | for (uint8_t axis = 0; axis < 3; ++axis) { |
||
184 | this->gyroscopeValue[axis] = global.l3g4200d.getAngularRate(axis);
|
||
185 | } |
||
186 | |||
187 | 58fe0e0b | Thomas Schöpping | return 0; |
188 | } |
||
189 | |||
190 | void DiWheelDrive::periodicBroadcast() {
|
||
191 | CANTxFrame frame; |
||
192 | frame.SID = 0;
|
||
193 | |||
194 | // Send the velocites µm/s of the x axis and µrad/s around z axis: start
|
||
195 | this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
|
||
196 | frame.data32[0] = this->actualSpeed[0]; |
||
197 | frame.data32[1] = this->actualSpeed[1]; |
||
198 | frame.DLC = 8;
|
||
199 | this->transmitMessage(&frame);
|
||
200 | |||
201 | // Send the valocites µm/s of the x axis and µrad/s around z axis: end
|
||
202 | // Send the odometry: start
|
||
203 | b4885314 | Thomas Schöpping | BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
204 | 58fe0e0b | Thomas Schöpping | // Set the frame id
|
205 | frame.SID = 0;
|
||
206 | this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
|
||
207 | // Cut of the first byte, which precission is not needed
|
||
208 | int32_t x_mm = (this->robotPosition.x >> 8); |
||
209 | int32_t y_mm = (this->robotPosition.y >> 8); |
||
210 | int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 ); |
||
211 | // Copy the data structure
|
||
212 | memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3); |
||
213 | memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3); |
||
214 | memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2); |
||
215 | frame.DLC = 8;
|
||
216 | this->transmitMessage(&frame);
|
||
217 | |||
218 | // Send the odometry: end
|
||
219 | // Send the proximity values of the floor: start
|
||
220 | b4885314 | Thomas Schöpping | BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
221 | 58fe0e0b | Thomas Schöpping | // Set the frame id
|
222 | frame.SID = 0;
|
||
223 | this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
|
||
224 | frame.data16[0] = this->proximityFloorValue[0]; |
||
225 | frame.data16[1] = this->proximityFloorValue[1]; |
||
226 | frame.data16[2] = this->proximityFloorValue[2]; |
||
227 | frame.data16[3] = this->proximityFloorValue[3]; |
||
228 | frame.DLC = 8;
|
||
229 | this->transmitMessage(&frame);
|
||
230 | |||
231 | b4885314 | Thomas Schöpping | // Send the magnetometer data
|
232 | for (uint8_t axis = 0; axis < 3; ++axis) { |
||
233 | frame.SID = 0;
|
||
234 | this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs |
||
235 | frame.data32[0] = this->magnetometerValue[axis]; |
||
236 | frame.DLC = 4;
|
||
237 | this->transmitMessage(&frame);
|
||
238 | } |
||
239 | |||
240 | // Send gyroscope data
|
||
241 | frame.SID = 0;
|
||
242 | this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID);
|
||
243 | frame.data16[0] = this->gyroscopeValue[0]; |
||
244 | frame.data16[1] = this->gyroscopeValue[1]; |
||
245 | frame.data16[2] = this->gyroscopeValue[2]; |
||
246 | frame.DLC = 6;
|
||
247 | this->transmitMessage(&frame);
|
||
248 | |||
249 | 58fe0e0b | Thomas Schöpping | // Send the board ID (board ID of DiWheelDrive = Robot ID)
|
250 | if (this->bcCounter % 10 == 0) { |
||
251 | frame.SID = 0;
|
||
252 | this->encodeDeviceId(&frame, CAN::ROBOT_ID);
|
||
253 | frame.data8[0] = this->robotId; |
||
254 | frame.DLC = 1;
|
||
255 | this->transmitMessage(&frame);
|
||
256 | } |
||
257 | |||
258 | ++this->bcCounter;
|
||
259 | } |
||
260 | |||
261 | void DiWheelDrive::calibrate() {
|
||
262 | // Stop sending and receiving of values to indicate the calibration phase
|
||
263 | // eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
|
||
264 | // rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
|
||
265 | |||
266 | this->calibrateProximityFloorValues();
|
||
267 | |||
268 | // Start sending and receving of values
|
||
269 | // eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
|
||
270 | // rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
|
||
271 | |||
272 | } |
||
273 | |||
274 | void DiWheelDrive::calibrateProximityFloorValues() {
|
||
275 | |||
276 | uint16_t buffer; |
||
277 | for (uint8_t idx = 0; idx < 4; ++idx) { |
||
278 | global.vcnl4020[idx].calibrate(); |
||
279 | buffer = global.vcnl4020[idx].getProximityOffset(); |
||
280 | global.memory.setVcnl4020Offset(buffer,idx); |
||
281 | } |
||
282 | |||
283 | } |
||
284 | |||
285 | ThreadReference DiWheelDrive::start(tprio_t PRIO) { |
||
286 | // set the robot ID as the board ID, which is read from the memory
|
||
287 | if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) { |
||
288 | this->robotId = 0; |
||
289 | } |
||
290 | |||
291 | this->ControllerAreaNetworkRx::start(PRIO + 1); |
||
292 | this->ControllerAreaNetworkTx::start(PRIO);
|
||
293 | return NULL; |
||
294 | } |
||
295 | |||
296 | msg_t |
||
297 | DiWheelDrive::terminate(void) {
|
||
298 | msg_t ret = RDY_OK; |
||
299 | |||
300 | this->ControllerAreaNetworkTx::requestTerminate();
|
||
301 | ret |= this->ControllerAreaNetworkTx::wait();
|
||
302 | this->ControllerAreaNetworkRx::requestTerminate();
|
||
303 | ret |= this->ControllerAreaNetworkRx::wait();
|
||
304 | |||
305 | return ret;
|
||
306 | } |