amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ f336542d
History | View | Annotate | Download (9.62 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 | |||
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 | b4885314 | Thomas Schöpping | // 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 | 58fe0e0b | Thomas Schöpping | 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 | 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 |
178 | 58fe0e0b | Thomas Schöpping | // 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 | 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 |
195 | 58fe0e0b | Thomas Schöpping | // 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 | b4885314 | Thomas Schöpping | // 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 | 58fe0e0b | Thomas Schöpping | // 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 | } |