Revision b4885314 devices/DiWheelDrive/DiWheelDrive.cpp
| devices/DiWheelDrive/DiWheelDrive.cpp | ||
|---|---|---|
| 148 | 148 |
for (int idx = 0; idx < 4; ++idx) |
| 149 | 149 |
this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset(); |
| 150 | 150 |
|
| 151 |
// Update magnetometer values |
|
| 152 |
for (uint8_t axis = 0; axis < 3; ++axis) {
|
|
| 153 |
this->magnetometerValue[axis] = global.hmc5883l.getMagnetizationGauss(axis); |
|
| 154 |
} |
|
| 155 |
|
|
| 156 |
// Update gyroscope values |
|
| 157 |
for (uint8_t axis = 0; axis < 3; ++axis) {
|
|
| 158 |
this->gyroscopeValue[axis] = global.l3g4200d.getAngularRate(axis); |
|
| 159 |
} |
|
| 160 |
|
|
| 151 | 161 |
return 0; |
| 152 | 162 |
} |
| 153 | 163 |
|
| ... | ... | |
| 164 | 174 |
|
| 165 | 175 |
// Send the valocites µm/s of the x axis and µrad/s around z axis: end |
| 166 | 176 |
// Send the odometry: start |
| 167 |
// BaseThread::sleep(MS2ST(10)); // Sleep, otherwise the cognition-board wont receive all messages
|
|
| 177 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee
|
|
| 168 | 178 |
// Set the frame id |
| 169 | 179 |
frame.SID = 0; |
| 170 | 180 |
this->encodeDeviceId(&frame, CAN::ODOMETRY_ID); |
| ... | ... | |
| 181 | 191 |
|
| 182 | 192 |
// Send the odometry: end |
| 183 | 193 |
// Send the proximity values of the floor: start |
| 184 |
// BaseThread::sleep(MS2ST(10)); // Sleep, otherwise the cognition-board wont receive all messages
|
|
| 194 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee
|
|
| 185 | 195 |
// Set the frame id |
| 186 | 196 |
frame.SID = 0; |
| 187 | 197 |
this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID); |
| ... | ... | |
| 192 | 202 |
frame.DLC = 8; |
| 193 | 203 |
this->transmitMessage(&frame); |
| 194 | 204 |
|
| 205 |
// Send the magnetometer data |
|
| 206 |
for (uint8_t axis = 0; axis < 3; ++axis) {
|
|
| 207 |
frame.SID = 0; |
|
| 208 |
this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs |
|
| 209 |
frame.data32[0] = this->magnetometerValue[axis]; |
|
| 210 |
frame.DLC = 4; |
|
| 211 |
this->transmitMessage(&frame); |
|
| 212 |
} |
|
| 213 |
|
|
| 214 |
// Send gyroscope data |
|
| 215 |
frame.SID = 0; |
|
| 216 |
this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID); |
|
| 217 |
frame.data16[0] = this->gyroscopeValue[0]; |
|
| 218 |
frame.data16[1] = this->gyroscopeValue[1]; |
|
| 219 |
frame.data16[2] = this->gyroscopeValue[2]; |
|
| 220 |
frame.DLC = 6; |
|
| 221 |
this->transmitMessage(&frame); |
|
| 222 |
|
|
| 195 | 223 |
// Send the board ID (board ID of DiWheelDrive = Robot ID) |
| 196 | 224 |
if (this->bcCounter % 10 == 0) {
|
| 197 | 225 |
frame.SID = 0; |
Also available in: Unified diff