Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / DiWheelDrive.cpp @ eef47799

History | View | Annotate | Download (11.2 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 == 8) {
102
        uint8_t speedForward    = frame->data8[0];
103
        uint8_t speedSoftLeft0  = frame->data8[1];
104
        uint8_t speedSoftLeft1  = frame->data8[2];
105
        uint8_t speedHardLeft0  = frame->data8[3];
106
        uint8_t speedHardLeft1  = frame->data8[4];
107
        global.rpmForward[0] = speedForward;
108
        global.rpmForward[1] = speedForward;
109
        global.rpmSoftLeft[0] = speedSoftLeft0;
110
        global.rpmSoftLeft[1] = speedSoftLeft1;
111
        global.rpmHardLeft[0] = speedHardLeft0;
112
        global.rpmHardLeft[1] = speedHardLeft1;
113
        global.rpmSoftRight[0] = global.rpmSoftLeft[1];
114
        global.rpmSoftRight[1] = global.rpmSoftLeft[0];
115
        global.rpmHardRight[0] = global.rpmHardLeft[1];
116
        global.rpmHardRight[1] = global.rpmHardLeft[0];
117
        return RDY_OK;
118
      }
119
      break;
120
    case CAN::SET_LINE_FOLLOW_MSG:
121
      // chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
122
      if (frame->DLC == 1) {
123
        global.lfStrategy = frame->data8[0];
124
        global.msgReceived = true;
125
        // return RDY_OK;
126
      }
127
      break;
128
    case CAN::SET_KINEMATIC_CONST_ID:
129
      if (frame->DLC == 8) {
130
/*        // Set (but do not store) Ed
131
        global.motorcontrol.setWheelDiameterCorrectionFactor(static_cast<float>(frame->data32[0]), false);
132
        // Set (but do not store) Eb
133
        global.motorcontrol.setActualWheelBaseDistance(static_cast<float>(frame->data32[1]), false);
134
        return RDY_OK;*/
135
        // Set (but do not store) Ed
136
        uint32_t ed_int = static_cast<uint32_t>(frame->data32[0]);
137
        float ed_float = static_cast<float>(ed_int)/1000000.0;
138
        global.motorcontrol.setWheelDiameterCorrectionFactor(ed_float, false);
139
        // Set (but do not store) Eb
140
        uint32_t eb_int = static_cast<uint32_t>(frame->data32[1]);
141
        float eb_float = static_cast<float>(eb_int)/1000000.0;
142
        global.motorcontrol.setActualWheelBaseDistance(eb_float, false);
143
        //chprintf((BaseSequentialStream*) &SD1, "Edi=%i, Edf=%f, Ebi=%i, Ebf=%f\n", ed_int, ed_float, eb_int, eb_float);
144
        return RDY_OK;
145
      }
146
      break;
147

    
148
    case CAN::POWER_STATUS_ID:
149
      if (frame->DLC == 6) {
150
        // The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
151
        types::power_status::ChargingState charging_flags;
152
        charging_flags.value = frame->data8[0];
153
        global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path);
154
        // Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
155
      }
156
    break;
157

    
158
    default:
159
      break;
160
  }
161
  return -1;
162
}
163

    
164
msg_t DiWheelDrive::updateSensorVal() {
165

    
166
  // Update robot velocity values
167
  kinematic currentVelocity = global.motorcontrol.getCurrentVelocity();
168
  this->actualSpeed[0] = currentVelocity.x;
169
  this->actualSpeed[1] = currentVelocity.w_z;
170

    
171
  // Update odometry values
172
  this->robotPosition = global.odometry.getPosition();
173

    
174
  // Update proximity values
175
  for (int idx = 0; idx < 4; ++idx)
176
    this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
177

    
178
  // Update magnetometer values
179
  for (uint8_t axis = 0; axis < 3; ++axis) {
180
    this->magnetometerValue[axis] = global.hmc5883l.getMagnetizationGauss(axis);
181
  }
182

    
183
  // Update gyroscope values
184
  for (uint8_t axis = 0; axis < 3; ++axis) {
185
    this->gyroscopeValue[axis] = global.l3g4200d.getAngularRate(axis);
186
  }
187

    
188
  return 0;
189
}
190

    
191
void DiWheelDrive::requestCharging(uint8_t power){
192
  CANTxFrame frame;
193
  frame.SID = 0;
194
  this->encodeDeviceId(&frame, CAN::REQUEST_CHARGING_OVER_PIN);
195
  frame.data8[0] = power;
196
  frame.DLC = 1;
197
  this->transmitMessage(&frame);
198
}
199

    
200
void DiWheelDrive::transmitState(uint8_t state){
201
  CANTxFrame frame;
202
  frame.SID = 0;
203
  this->encodeDeviceId(&frame, CAN::TRANSMIT_LINE_FOLLOW_STATE);
204
  frame.data8[0] = state;
205
  frame.DLC = 1;
206
  this->transmitMessage(&frame);
207
}
208

    
209
void DiWheelDrive::periodicBroadcast() {
210
  CANTxFrame frame;
211
  frame.SID = 0;
212

    
213
  // Send the velocites µm/s of the x axis and µrad/s around z axis: start
214
  this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
215
  frame.data32[0] = this->actualSpeed[0];
216
  frame.data32[1] = this->actualSpeed[1];
217
  frame.DLC = 8;
218
  this->transmitMessage(&frame);
219

    
220
  // Send the valocites µm/s of the x axis and µrad/s around z axis: end
221
  // Send the odometry: start
222
  BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee
223
  // Set the frame id
224
  frame.SID = 0;
225
  this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
226
  // Cut of the first byte, which precission is not needed
227
  int32_t x_mm = (this->robotPosition.x >> 8);
228
  int32_t y_mm = (this->robotPosition.y >> 8);
229
  int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 );
230
  // Copy the data structure
231
  memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3);
232
  memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3);
233
  memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2);
234
  frame.DLC = 8;
235
  this->transmitMessage(&frame);
236

    
237
  // Send the odometry: end
238
  // Send the proximity values of the floor: start
239
  BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee
240
  // Set the frame id
241
  frame.SID = 0;
242
  this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
243
  frame.data16[0] = this->proximityFloorValue[0];
244
  frame.data16[1] = this->proximityFloorValue[1];
245
  frame.data16[2] = this->proximityFloorValue[2];
246
  frame.data16[3] = this->proximityFloorValue[3];
247
  frame.DLC = 8;
248
  this->transmitMessage(&frame);
249

    
250
  // Send the magnetometer data
251
  for (uint8_t axis = 0; axis < 3; ++axis) {
252
    frame.SID = 0;
253
    this->encodeDeviceId(&frame, CAN::MAGNETOMETER_X_ID + axis); // Y- and Z-axis have according IDs
254
    frame.data32[0] = this->magnetometerValue[axis];
255
    frame.DLC = 4;
256
    this->transmitMessage(&frame);
257
  }
258

    
259
  // Send gyroscope data
260
  frame.SID = 0;
261
  this->encodeDeviceId(&frame, CAN::GYROSCOPE_ID);
262
  frame.data16[0] = this->gyroscopeValue[0];
263
  frame.data16[1] = this->gyroscopeValue[1];
264
  frame.data16[2] = this->gyroscopeValue[2];
265
  frame.DLC = 6;
266
  this->transmitMessage(&frame);
267

    
268
  // Send the board ID (board ID of DiWheelDrive = Robot ID)
269
  if (this->bcCounter % 10 == 0) {
270
    frame.SID = 0;
271
    this->encodeDeviceId(&frame, CAN::ROBOT_ID);
272
    frame.data8[0] = this->robotId;
273
    frame.DLC = 1;
274
    this->transmitMessage(&frame);
275
  }
276

    
277
  ++this->bcCounter;
278
}
279

    
280
void DiWheelDrive::calibrate() {
281
  // Stop sending and receiving of values to indicate the calibration phase
282
//   eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
283
//   rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
284

    
285
  this->calibrateProximityFloorValues();
286

    
287
  // Start sending and receving of values
288
//   eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
289
//   rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
290

    
291
}
292

    
293
void DiWheelDrive::calibrateProximityFloorValues() {
294

    
295
  uint16_t buffer;
296
  for (uint8_t idx = 0; idx < 4; ++idx) {
297
    global.vcnl4020[idx].calibrate();
298
    buffer = global.vcnl4020[idx].getProximityOffset();
299
    global.memory.setVcnl4020Offset(buffer,idx);
300
  }
301

    
302
}
303

    
304
ThreadReference DiWheelDrive::start(tprio_t PRIO) {
305
  // set the robot ID as the board ID, which is read from the memory
306
  if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) {
307
    this->robotId = 0;
308
  }
309

    
310
  this->ControllerAreaNetworkRx::start(PRIO + 1);
311
  this->ControllerAreaNetworkTx::start(PRIO);
312
  return NULL;
313
}
314

    
315
msg_t
316
DiWheelDrive::terminate(void) {
317
  msg_t ret = RDY_OK;
318

    
319
  this->ControllerAreaNetworkTx::requestTerminate();
320
  ret |= this->ControllerAreaNetworkTx::wait();
321
  this->ControllerAreaNetworkRx::requestTerminate();
322
  ret |= this->ControllerAreaNetworkRx::wait();
323

    
324
  return ret;
325
}