Statistics
| Branch: | Tag: | Revision:

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
}