Statistics
| Branch: | Tag: | Revision:

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
}