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