amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ f336542d
History | View | Annotate | Download (9.62 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 |
|
101 |
case CAN::SET_KINEMATIC_CONST_ID:
|
102 |
if (frame->DLC == 8) { |
103 |
/* // Set (but do not store) Ed
|
104 |
global.motorcontrol.setWheelDiameterCorrectionFactor(static_cast<float>(frame->data32[0]), false);
|
105 |
// Set (but do not store) Eb
|
106 |
global.motorcontrol.setActualWheelBaseDistance(static_cast<float>(frame->data32[1]), false);
|
107 |
return RDY_OK;*/
|
108 |
// Set (but do not store) Ed
|
109 |
uint32_t ed_int = static_cast<uint32_t>(frame->data32[0]); |
110 |
float ed_float = static_cast<float>(ed_int)/1000000.0; |
111 |
global.motorcontrol.setWheelDiameterCorrectionFactor(ed_float, false);
|
112 |
// Set (but do not store) Eb
|
113 |
uint32_t eb_int = static_cast<uint32_t>(frame->data32[1]); |
114 |
float eb_float = static_cast<float>(eb_int)/1000000.0; |
115 |
global.motorcontrol.setActualWheelBaseDistance(eb_float, false);
|
116 |
//chprintf((BaseSequentialStream*) &SD1, "Edi=%i, Edf=%f, Ebi=%i, Ebf=%f\n", ed_int, ed_float, eb_int, eb_float);
|
117 |
return RDY_OK;
|
118 |
} |
119 |
break;
|
120 |
|
121 |
case CAN::POWER_STATUS_ID:
|
122 |
if (frame->DLC == 6) { |
123 |
// The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
|
124 |
types::power_status::ChargingState charging_flags; |
125 |
charging_flags.value = frame->data8[0];
|
126 |
global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path); |
127 |
// Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
|
128 |
} |
129 |
break;
|
130 |
|
131 |
default:
|
132 |
break;
|
133 |
} |
134 |
return -1; |
135 |
} |
136 |
|
137 |
msg_t DiWheelDrive::updateSensorVal() { |
138 |
|
139 |
// Update robot velocity values
|
140 |
kinematic currentVelocity = global.motorcontrol.getCurrentVelocity(); |
141 |
this->actualSpeed[0] = currentVelocity.x; |
142 |
this->actualSpeed[1] = currentVelocity.w_z; |
143 |
|
144 |
// Update odometry values
|
145 |
this->robotPosition = global.odometry.getPosition();
|
146 |
|
147 |
// Update proximity values
|
148 |
for (int idx = 0; idx < 4; ++idx) |
149 |
this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
|
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 |
|
161 |
return 0; |
162 |
} |
163 |
|
164 |
void DiWheelDrive::periodicBroadcast() {
|
165 |
CANTxFrame frame; |
166 |
frame.SID = 0;
|
167 |
|
168 |
// Send the velocites µm/s of the x axis and µrad/s around z axis: start
|
169 |
this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
|
170 |
frame.data32[0] = this->actualSpeed[0]; |
171 |
frame.data32[1] = this->actualSpeed[1]; |
172 |
frame.DLC = 8;
|
173 |
this->transmitMessage(&frame);
|
174 |
|
175 |
// Send the valocites µm/s of the x axis and µrad/s around z axis: end
|
176 |
// Send the odometry: start
|
177 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
178 |
// Set the frame id
|
179 |
frame.SID = 0;
|
180 |
this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
|
181 |
// Cut of the first byte, which precission is not needed
|
182 |
int32_t x_mm = (this->robotPosition.x >> 8); |
183 |
int32_t y_mm = (this->robotPosition.y >> 8); |
184 |
int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 ); |
185 |
// Copy the data structure
|
186 |
memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3); |
187 |
memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3); |
188 |
memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2); |
189 |
frame.DLC = 8;
|
190 |
this->transmitMessage(&frame);
|
191 |
|
192 |
// Send the odometry: end
|
193 |
// Send the proximity values of the floor: start
|
194 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
195 |
// Set the frame id
|
196 |
frame.SID = 0;
|
197 |
this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
|
198 |
frame.data16[0] = this->proximityFloorValue[0]; |
199 |
frame.data16[1] = this->proximityFloorValue[1]; |
200 |
frame.data16[2] = this->proximityFloorValue[2]; |
201 |
frame.data16[3] = this->proximityFloorValue[3]; |
202 |
frame.DLC = 8;
|
203 |
this->transmitMessage(&frame);
|
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 |
|
223 |
// Send the board ID (board ID of DiWheelDrive = Robot ID)
|
224 |
if (this->bcCounter % 10 == 0) { |
225 |
frame.SID = 0;
|
226 |
this->encodeDeviceId(&frame, CAN::ROBOT_ID);
|
227 |
frame.data8[0] = this->robotId; |
228 |
frame.DLC = 1;
|
229 |
this->transmitMessage(&frame);
|
230 |
} |
231 |
|
232 |
++this->bcCounter;
|
233 |
} |
234 |
|
235 |
void DiWheelDrive::calibrate() {
|
236 |
// Stop sending and receiving of values to indicate the calibration phase
|
237 |
// eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
|
238 |
// rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
|
239 |
|
240 |
this->calibrateProximityFloorValues();
|
241 |
|
242 |
// Start sending and receving of values
|
243 |
// eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
|
244 |
// rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
|
245 |
|
246 |
} |
247 |
|
248 |
void DiWheelDrive::calibrateProximityFloorValues() {
|
249 |
|
250 |
uint16_t buffer; |
251 |
for (uint8_t idx = 0; idx < 4; ++idx) { |
252 |
global.vcnl4020[idx].calibrate(); |
253 |
buffer = global.vcnl4020[idx].getProximityOffset(); |
254 |
global.memory.setVcnl4020Offset(buffer,idx); |
255 |
} |
256 |
|
257 |
} |
258 |
|
259 |
ThreadReference DiWheelDrive::start(tprio_t PRIO) { |
260 |
// set the robot ID as the board ID, which is read from the memory
|
261 |
if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) { |
262 |
this->robotId = 0; |
263 |
} |
264 |
|
265 |
this->ControllerAreaNetworkRx::start(PRIO + 1); |
266 |
this->ControllerAreaNetworkTx::start(PRIO);
|
267 |
return NULL; |
268 |
} |
269 |
|
270 |
msg_t |
271 |
DiWheelDrive::terminate(void) {
|
272 |
msg_t ret = RDY_OK; |
273 |
|
274 |
this->ControllerAreaNetworkTx::requestTerminate();
|
275 |
ret |= this->ControllerAreaNetworkTx::wait();
|
276 |
this->ControllerAreaNetworkRx::requestTerminate();
|
277 |
ret |= this->ControllerAreaNetworkRx::wait();
|
278 |
|
279 |
return ret;
|
280 |
} |