Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ 309980f0

History | View | Annotate | Download (11.2 KB)

1 58fe0e0b Thomas Schöpping
#include "ch.hpp"
2
#include "hal.h"
3
#include "qei.h"
4
#include "DiWheelDrive.h"
5 9c46b728 galberding
6 58fe0e0b Thomas Schöpping
#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 0f37fb41 galberding
      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 9c46b728 galberding
        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 d607fcef galberding
    case CAN::SET_LINE_FOLLOW_MSG:
116 8c99e03a galberding
      // chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
117 9c46b728 galberding
      if (frame->DLC == 1) {
118
        global.lfStrategy = frame->data8[0];
119 d607fcef galberding
        global.msgReceived = true;
120 8c99e03a galberding
        // return RDY_OK;
121 9c46b728 galberding
      }
122
      break;
123 58fe0e0b Thomas Schöpping
    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 b4885314 Thomas Schöpping
  // 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 58fe0e0b Thomas Schöpping
  return 0;
184
}
185
186 8c99e03a galberding
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 84b4c632 galberding
void DiWheelDrive::transmitState(int8_t state){
196 3a4c95b0 galberding
  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 309980f0 galberding
void DiWheelDrive::transmitMapState(map_state state) {
205
  CANTxFrame frame;
206
  // frame.SID = 0;
207
  // this->encodeDeviceId(&frame, CAN::TRANSMIT_LINE_FOLLOW_STATE);
208
  // frame.data8[0] = state;
209
  // frame.DLC = 1;
210
  // this->transmitMessage(&frame);
211
}
212
213 58fe0e0b Thomas Schöpping
void DiWheelDrive::periodicBroadcast() {
214
  CANTxFrame frame;
215
  frame.SID = 0;
216
217
  // Send the velocites µm/s of the x axis and µrad/s around z axis: start
218
  this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
219
  frame.data32[0] = this->actualSpeed[0];
220
  frame.data32[1] = this->actualSpeed[1];
221
  frame.DLC = 8;
222
  this->transmitMessage(&frame);
223
224
  // Send the valocites µm/s of the x axis and µrad/s around z axis: end
225
  // Send the odometry: start
226 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
227 58fe0e0b Thomas Schöpping
  // Set the frame id
228
  frame.SID = 0;
229
  this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
230
  // Cut of the first byte, which precission is not needed
231
  int32_t x_mm = (this->robotPosition.x >> 8);
232
  int32_t y_mm = (this->robotPosition.y >> 8);
233
  int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 );
234
  // Copy the data structure
235
  memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3);
236
  memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3);
237
  memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2);
238
  frame.DLC = 8;
239
  this->transmitMessage(&frame);
240
241
  // Send the odometry: end
242
  // Send the proximity values of the floor: start
243 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
244 58fe0e0b Thomas Schöpping
  // Set the frame id
245
  frame.SID = 0;
246
  this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
247
  frame.data16[0] = this->proximityFloorValue[0];
248
  frame.data16[1] = this->proximityFloorValue[1];
249
  frame.data16[2] = this->proximityFloorValue[2];
250
  frame.data16[3] = this->proximityFloorValue[3];
251
  frame.DLC = 8;
252
  this->transmitMessage(&frame);
253
254 b4885314 Thomas Schöpping
  // Send the magnetometer data
255
  for (uint8_t axis = 0; axis < 3; ++axis) {
256
    frame.SID = 0;
257
    this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs
258
    frame.data32[0] = this->magnetometerValue[axis];
259
    frame.DLC = 4;
260
    this->transmitMessage(&frame);
261
  }
262
263
  // Send gyroscope data
264
  frame.SID = 0;
265
  this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID);
266
  frame.data16[0] = this->gyroscopeValue[0];
267
  frame.data16[1] = this->gyroscopeValue[1];
268
  frame.data16[2] = this->gyroscopeValue[2];
269
  frame.DLC = 6;
270
  this->transmitMessage(&frame);
271
272 58fe0e0b Thomas Schöpping
  // Send the board ID (board ID of DiWheelDrive = Robot ID)
273
  if (this->bcCounter % 10 == 0) {
274
    frame.SID = 0;
275
    this->encodeDeviceId(&frame, CAN::ROBOT_ID);
276
    frame.data8[0] = this->robotId;
277
    frame.DLC = 1;
278
    this->transmitMessage(&frame);
279
  }
280
281
  ++this->bcCounter;
282
}
283
284
void DiWheelDrive::calibrate() {
285
  // Stop sending and receiving of values to indicate the calibration phase
286
//   eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
287
//   rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
288
289
  this->calibrateProximityFloorValues();
290
291
  // Start sending and receving of values
292
//   eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
293
//   rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
294
295
}
296
297
void DiWheelDrive::calibrateProximityFloorValues() {
298
299
  uint16_t buffer;
300
  for (uint8_t idx = 0; idx < 4; ++idx) {
301
    global.vcnl4020[idx].calibrate();
302
    buffer = global.vcnl4020[idx].getProximityOffset();
303
    global.memory.setVcnl4020Offset(buffer,idx);
304
  }
305
306
}
307
308
ThreadReference DiWheelDrive::start(tprio_t PRIO) {
309
  // set the robot ID as the board ID, which is read from the memory
310
  if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) {
311
    this->robotId = 0;
312
  }
313
314
  this->ControllerAreaNetworkRx::start(PRIO + 1);
315
  this->ControllerAreaNetworkTx::start(PRIO);
316
  return NULL;
317
}
318
319
msg_t
320
DiWheelDrive::terminate(void) {
321
  msg_t ret = RDY_OK;
322
323
  this->ControllerAreaNetworkTx::requestTerminate();
324
  ret |= this->ControllerAreaNetworkTx::wait();
325
  this->ControllerAreaNetworkRx::requestTerminate();
326
  ret |= this->ControllerAreaNetworkRx::wait();
327
328
  return ret;
329
}