amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ 0f37fb41
History | View | Annotate | Download (10.9 KB)
1 |
#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 |
case CAN::SET_LINE_FOLLOW_SPEED:
|
101 |
if (frame->DLC == 5) { |
102 |
global.rpmForward[0] = frame->data8[0]; |
103 |
global.rpmForward[1] = frame->data8[0]; |
104 |
global.rpmSoftLeft[0] = frame->data8[1]; |
105 |
global.rpmSoftLeft[1] = frame->data8[2]; |
106 |
global.rpmHardLeft[0] = frame->data8[3]; |
107 |
global.rpmHardLeft[1] = frame->data8[4]; |
108 |
global.rpmSoftRight[0] = global.rpmSoftLeft[1]; |
109 |
global.rpmSoftRight[1] = global.rpmSoftLeft[0]; |
110 |
global.rpmHardRight[0] = global.rpmHardLeft[1]; |
111 |
global.rpmHardRight[1] = global.rpmHardLeft[0]; |
112 |
return RDY_OK;
|
113 |
} |
114 |
break;
|
115 |
case CAN::SET_LINE_FOLLOW_MSG:
|
116 |
// chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
|
117 |
if (frame->DLC == 1) { |
118 |
global.lfStrategy = frame->data8[0];
|
119 |
global.msgReceived = true;
|
120 |
// return RDY_OK;
|
121 |
} |
122 |
break;
|
123 |
case CAN::SET_KINEMATIC_CONST_ID:
|
124 |
if (frame->DLC == 8) { |
125 |
/* // Set (but do not store) Ed
|
126 |
global.motorcontrol.setWheelDiameterCorrectionFactor(static_cast<float>(frame->data32[0]), false);
|
127 |
// Set (but do not store) Eb
|
128 |
global.motorcontrol.setActualWheelBaseDistance(static_cast<float>(frame->data32[1]), false);
|
129 |
return RDY_OK;*/
|
130 |
// Set (but do not store) Ed
|
131 |
uint32_t ed_int = static_cast<uint32_t>(frame->data32[0]); |
132 |
float ed_float = static_cast<float>(ed_int)/1000000.0; |
133 |
global.motorcontrol.setWheelDiameterCorrectionFactor(ed_float, false);
|
134 |
// Set (but do not store) Eb
|
135 |
uint32_t eb_int = static_cast<uint32_t>(frame->data32[1]); |
136 |
float eb_float = static_cast<float>(eb_int)/1000000.0; |
137 |
global.motorcontrol.setActualWheelBaseDistance(eb_float, false);
|
138 |
//chprintf((BaseSequentialStream*) &SD1, "Edi=%i, Edf=%f, Ebi=%i, Ebf=%f\n", ed_int, ed_float, eb_int, eb_float);
|
139 |
return RDY_OK;
|
140 |
} |
141 |
break;
|
142 |
|
143 |
case CAN::POWER_STATUS_ID:
|
144 |
if (frame->DLC == 6) { |
145 |
// The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
|
146 |
types::power_status::ChargingState charging_flags; |
147 |
charging_flags.value = frame->data8[0];
|
148 |
global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path); |
149 |
// Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
|
150 |
} |
151 |
break;
|
152 |
|
153 |
default:
|
154 |
break;
|
155 |
} |
156 |
return -1; |
157 |
} |
158 |
|
159 |
msg_t DiWheelDrive::updateSensorVal() { |
160 |
|
161 |
// Update robot velocity values
|
162 |
kinematic currentVelocity = global.motorcontrol.getCurrentVelocity(); |
163 |
this->actualSpeed[0] = currentVelocity.x; |
164 |
this->actualSpeed[1] = currentVelocity.w_z; |
165 |
|
166 |
// Update odometry values
|
167 |
this->robotPosition = global.odometry.getPosition();
|
168 |
|
169 |
// Update proximity values
|
170 |
for (int idx = 0; idx < 4; ++idx) |
171 |
this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
|
172 |
|
173 |
// Update magnetometer values
|
174 |
for (uint8_t axis = 0; axis < 3; ++axis) { |
175 |
this->magnetometerValue[axis] = global.hmc5883l.getMagnetizationGauss(axis);
|
176 |
} |
177 |
|
178 |
// Update gyroscope values
|
179 |
for (uint8_t axis = 0; axis < 3; ++axis) { |
180 |
this->gyroscopeValue[axis] = global.l3g4200d.getAngularRate(axis);
|
181 |
} |
182 |
|
183 |
return 0; |
184 |
} |
185 |
|
186 |
void DiWheelDrive::requestCharging(uint8_t power){
|
187 |
CANTxFrame frame; |
188 |
frame.SID = 0;
|
189 |
this->encodeDeviceId(&frame, CAN::REQUEST_CHARGING_OVER_PIN);
|
190 |
frame.data8[0] = power;
|
191 |
frame.DLC = 1;
|
192 |
this->transmitMessage(&frame);
|
193 |
} |
194 |
|
195 |
void DiWheelDrive::transmitState(uint8_t state){
|
196 |
CANTxFrame frame; |
197 |
frame.SID = 0;
|
198 |
this->encodeDeviceId(&frame, CAN::TRANSMIT_LINE_FOLLOW_STATE);
|
199 |
frame.data8[0] = state;
|
200 |
frame.DLC = 1;
|
201 |
this->transmitMessage(&frame);
|
202 |
} |
203 |
|
204 |
void DiWheelDrive::periodicBroadcast() {
|
205 |
CANTxFrame frame; |
206 |
frame.SID = 0;
|
207 |
|
208 |
// Send the velocites µm/s of the x axis and µrad/s around z axis: start
|
209 |
this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
|
210 |
frame.data32[0] = this->actualSpeed[0]; |
211 |
frame.data32[1] = this->actualSpeed[1]; |
212 |
frame.DLC = 8;
|
213 |
this->transmitMessage(&frame);
|
214 |
|
215 |
// Send the valocites µm/s of the x axis and µrad/s around z axis: end
|
216 |
// Send the odometry: start
|
217 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
218 |
// Set the frame id
|
219 |
frame.SID = 0;
|
220 |
this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
|
221 |
// Cut of the first byte, which precission is not needed
|
222 |
int32_t x_mm = (this->robotPosition.x >> 8); |
223 |
int32_t y_mm = (this->robotPosition.y >> 8); |
224 |
int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 ); |
225 |
// Copy the data structure
|
226 |
memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3); |
227 |
memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3); |
228 |
memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2); |
229 |
frame.DLC = 8;
|
230 |
this->transmitMessage(&frame);
|
231 |
|
232 |
// Send the odometry: end
|
233 |
// Send the proximity values of the floor: start
|
234 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
235 |
// Set the frame id
|
236 |
frame.SID = 0;
|
237 |
this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
|
238 |
frame.data16[0] = this->proximityFloorValue[0]; |
239 |
frame.data16[1] = this->proximityFloorValue[1]; |
240 |
frame.data16[2] = this->proximityFloorValue[2]; |
241 |
frame.data16[3] = this->proximityFloorValue[3]; |
242 |
frame.DLC = 8;
|
243 |
this->transmitMessage(&frame);
|
244 |
|
245 |
// Send the magnetometer data
|
246 |
for (uint8_t axis = 0; axis < 3; ++axis) { |
247 |
frame.SID = 0;
|
248 |
this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs |
249 |
frame.data32[0] = this->magnetometerValue[axis]; |
250 |
frame.DLC = 4;
|
251 |
this->transmitMessage(&frame);
|
252 |
} |
253 |
|
254 |
// Send gyroscope data
|
255 |
frame.SID = 0;
|
256 |
this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID);
|
257 |
frame.data16[0] = this->gyroscopeValue[0]; |
258 |
frame.data16[1] = this->gyroscopeValue[1]; |
259 |
frame.data16[2] = this->gyroscopeValue[2]; |
260 |
frame.DLC = 6;
|
261 |
this->transmitMessage(&frame);
|
262 |
|
263 |
// Send the board ID (board ID of DiWheelDrive = Robot ID)
|
264 |
if (this->bcCounter % 10 == 0) { |
265 |
frame.SID = 0;
|
266 |
this->encodeDeviceId(&frame, CAN::ROBOT_ID);
|
267 |
frame.data8[0] = this->robotId; |
268 |
frame.DLC = 1;
|
269 |
this->transmitMessage(&frame);
|
270 |
} |
271 |
|
272 |
++this->bcCounter;
|
273 |
} |
274 |
|
275 |
void DiWheelDrive::calibrate() {
|
276 |
// Stop sending and receiving of values to indicate the calibration phase
|
277 |
// eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
|
278 |
// rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
|
279 |
|
280 |
this->calibrateProximityFloorValues();
|
281 |
|
282 |
// Start sending and receving of values
|
283 |
// eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
|
284 |
// rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
|
285 |
|
286 |
} |
287 |
|
288 |
void DiWheelDrive::calibrateProximityFloorValues() {
|
289 |
|
290 |
uint16_t buffer; |
291 |
for (uint8_t idx = 0; idx < 4; ++idx) { |
292 |
global.vcnl4020[idx].calibrate(); |
293 |
buffer = global.vcnl4020[idx].getProximityOffset(); |
294 |
global.memory.setVcnl4020Offset(buffer,idx); |
295 |
} |
296 |
|
297 |
} |
298 |
|
299 |
ThreadReference DiWheelDrive::start(tprio_t PRIO) { |
300 |
// set the robot ID as the board ID, which is read from the memory
|
301 |
if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) { |
302 |
this->robotId = 0; |
303 |
} |
304 |
|
305 |
this->ControllerAreaNetworkRx::start(PRIO + 1); |
306 |
this->ControllerAreaNetworkTx::start(PRIO);
|
307 |
return NULL; |
308 |
} |
309 |
|
310 |
msg_t |
311 |
DiWheelDrive::terminate(void) {
|
312 |
msg_t ret = RDY_OK; |
313 |
|
314 |
this->ControllerAreaNetworkTx::requestTerminate();
|
315 |
ret |= this->ControllerAreaNetworkTx::wait();
|
316 |
this->ControllerAreaNetworkRx::requestTerminate();
|
317 |
ret |= this->ControllerAreaNetworkRx::wait();
|
318 |
|
319 |
return ret;
|
320 |
} |