Statistics
| Branch: | Tag: | Revision:

amiro-os / components / Odometry.cpp @ f336542d

History | View | Annotate | Download (8.19 KB)

1
#include <ch.hpp>
2
#include <hal.h>
3

    
4
#include <qei.h>
5

    
6
#include <amiro/Odometry.h>
7

    
8
#include <math.h> // cos(), sin()
9
#include <Matrix.h> // Matrixoperations "Matrix::*"
10
#include <amiro/Constants.h> // Constants "constants::*"
11
#include <chprintf.h>
12
#include <global.hpp>
13

    
14
using namespace chibios_rt;
15
using namespace amiro;
16
using namespace constants::DiWheelDrive;
17

    
18
extern Global global;
19

    
20

    
21
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)
22
    : BaseStaticThread<512>(),
23
      motorIncrements(mi),
24
      gyro(gyroscope),
25
      eventSource(),
26
      period(50),
27
      incrementsPerRevolution(incrementsPerRevolution),
28
      updatesPerMinute(constants::secondsPerMinute * constants::millisecondsPerSecond / this->period),
29
      wheelCircumference(wheelCircumferenceSI),
30
      wheelBaseDistanceSI(wheelBaseDistanceSI) {
31

    
32

    
33
//  this-> = constants::secondsPerMinute * constants::millisecondsPerSecond / this->period;
34
//  this->wheelCircumference = constants::wheelCircumferenceSI;
35
//  this->wheelBaseDistanceSI = constants::wheelBaseDistanceSI;
36

    
37
  this->distance[LEFT_WHEEL] = 0.0f;
38
  this->distance[RIGHT_WHEEL] = 0.0f;
39
  this->increment[LEFT_WHEEL] = 0;
40
  this->increment[RIGHT_WHEEL] = 0;
41
  this->incrementDifference[LEFT_WHEEL] = 0.0f;
42
  this->incrementDifference[RIGHT_WHEEL] = 0.0f;
43
  this->distance[LEFT_WHEEL] = 0.0f;
44
  this->distance[RIGHT_WHEEL] = 0.0f;
45

    
46
  this->wheelError[LEFT_WHEEL] = wheelErrorSI[LEFT_WHEEL];
47
  this->wheelError[RIGHT_WHEEL] = wheelErrorSI[RIGHT_WHEEL];
48

    
49
  this->resetPosition(); // Init position
50

    
51
  this->resetError(); // Init error Cp
52

    
53
}
54

    
55
types::position Odometry::getPosition() {
56
  types::position robotPosition;
57
  const int32_t piScaled = int32_t(2 * M_PI * 1e6);
58
  chSysLock();
59
    // Conversion from standard unit to ยต unit
60
    robotPosition.x = this->pX * 1e6;
61
    robotPosition.y = this->pY * 1e6;
62
    robotPosition.f_z = (int32_t(this->pPhi * 1e6) % piScaled) + ((this->pPhi < 0) ? piScaled : 0);  // Get only the postitve angel f_z in [0 .. 2 * pi]
63
  chSysUnlock();
64
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);
65
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
66
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
67
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
68
  return robotPosition;
69
}
70

    
71
void Odometry::setPosition(float pX, float pY, float pPhi) {
72
  chSysLock();
73
    this->pX = pX;
74
    this->pY = pY;
75
    this->pPhi = pPhi;
76
  chSysUnlock();
77
}
78

    
79
void Odometry::resetPosition() {
80
  this->setPosition(0.0f,0.0f,0.0f);
81
}
82

    
83
void Odometry::setError(float* Cp3x3) {
84
  chSysLock();
85
    Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
86
  chSysUnlock();
87
}
88

    
89
void Odometry::resetError() {
90
  Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);
91
}
92

    
93
EvtSource* Odometry::getEventSource() {
94
  return &this->eventSource;
95
}
96

    
97
msg_t Odometry::main(void) {
98
  systime_t time = System::getTime();
99
  this->setName("Odometry");
100

    
101
  while (!this->shouldTerminate()) {
102
    time += MS2ST(this->period);
103

    
104
    // Update the base distance, because it may have changed after a calibration
105
    this->updateWheelBaseDistance();
106

    
107
    // Get the actual speed
108
    this->updateDistance();
109

    
110
    // Calculate the odometry
111
    this->updateOdometry();
112

    
113

    
114
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
115
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
116
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);
117
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
118

    
119
    if (time >= System::getTime()) {
120
        chThdSleepUntil(time);
121
    } else {
122
        chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
123
    }
124
  }
125

    
126
  return true;
127
}
128

    
129
void Odometry::updateOdometry() {
130

    
131
  // Get the temporary position and error
132
  float Cp3x3[9];
133
  int32_t angular_ud;
134
  int32_t angularRate_udps;
135
  chSysLock();
136
    float pX = this->pX;
137
    float pY = this->pY;
138
    float pPhi = this->pPhi;
139
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
140
    // TODO Get the gyro (or gyro rate) information and do something with it
141
    // angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
142
    // angularRate_udps = gyro->getAngularRate_udps(L3G4200D::AXIS_Z);
143
  chSysUnlock();
144

    
145
  ////////////////
146
  // Temporary calculations
147
  ////////////////
148

    
149
  // TMP: Rotated angular
150
  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
151
  // TODO Calculate the differential angel dPhi from either the angular (1. line) or angular rate (2.+3. line)
152
  // float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
153
  // const float angular_md = float((angularRate_udps * this->period / constants::millisecondsPerSecond) * 1e-3);
154
  // float dPhi = ((angular_md * M_PI) * 1e-3) / 180.0f;
155

    
156
  // TMP: Moved distance
157
  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
158

    
159
  // TMP: Argument for the trigonometric functions
160
  float trigArg = pPhi + dPhi / 2.0f;
161

    
162
  // TMP: Trigonometric functions
163
  float cosArg = cos(trigArg);
164
  float sinArg = sin(trigArg);
165

    
166
  // TMP: Delta distance
167
  float dPX = dDistance * cosArg;
168
  float dPY = dDistance * sinArg;
169

    
170
  ////////////////
171
  // Position Update
172
  ////////////////
173

    
174
  // Update distance
175
  pX += dPX;
176
  pY += dPY;
177
  pPhi += dPhi;
178

    
179
  ////////////////
180
  // Temporary error calculations
181
  ////////////////
182

    
183
  // position propagation error (3x3 matrix)
184
  float Fp3x3[9]  = {1.0f, 0.0f, -dPY,
185
                     0.0f, 1.0f,  dPX,
186
                     0.0f, 0.0f, 1.0f};
187
  // steering error (2x2 matrix)
188
  float Cs2x2[4] = {abs(this->distance[RIGHT_WHEEL])*wheelError[RIGHT_WHEEL],0.0f,
189
                    0.0f, abs(this->distance[LEFT_WHEEL])*wheelError[LEFT_WHEEL]};
190
  // steering propagation error (3x2 matrix)
191
  float Fs3x2[6] = {(cosArg+dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f, (sinArg+dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f,
192
                 (sinArg-dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f, (cosArg-dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f,
193
                 -1.0f/this->wheelBaseDistanceSI                         , 1.0f/this->wheelBaseDistanceSI};
194

    
195
  ////////////////
196
  // Error calculations tmpCp = Fp*Cp*~Fp
197
  ////////////////
198
  // New position error
199
  float tmpCp3x3[9] = {0.0f};
200
  float tmpFpCp3x3[9] = {0.0f};
201
  // tmpFpCp = Fp*Cp
202
  Matrix::XdotY<float>(&(Fp3x3[0]),3,3,&(Cp3x3[0]),3,3,&(tmpFpCp3x3[0]),3,3);
203
  // tmpCp = tmpFpCp*~Fp
204
  Matrix::XdotYtrans<float>(&(tmpFpCp3x3[0]),3,3,&(Fp3x3[0]),3,3,&(tmpCp3x3[0]),3,3);
205

    
206
  ////////////////
207
  // Error calculations tmpCs = Fs*Cs*~Fs
208
  ////////////////
209
  // New steering error
210
  float tmpCs3x3[9] = {0.0f};
211
  float tmpFsCs3x2[6] = {0.0f};
212
  // tmpFsCs = Fs*Cs
213
  Matrix::XdotY<float>(&(Fs3x2[0]),3,2,&(Cs2x2[0]),2,2,&(tmpFsCs3x2[0]),3,2);
214
  // tmpCs = tmpFsCs*~Fs
215
  Matrix::XdotYtrans<float>(&(tmpFsCs3x2[0]),3,2,&(Fs3x2[0]),3,2,&(tmpCs3x3[0]),3,3);
216

    
217
  ////////////////
218
  // Error calculations Cp = Fp*Cp*~Fp + Fs*Cs*~Fs
219
  ////////////////
220
  Matrix::XplusY<float>(tmpCp3x3,3,3,tmpCs3x3,3,3,Cp3x3,3,3);
221

    
222
  ////////////////
223
  // Write back
224
  ////////////////
225

    
226
  // Write back
227
  this->setPosition(pX,pY,pPhi);
228
  chSysLock();
229
    Matrix::copy<float>(Cp3x3,3,3,this->Cp3x3,3,3);
230
  chSysUnlock();
231

    
232
}
233

    
234
void Odometry::updateWheelBaseDistance() {
235
  this->wheelBaseDistanceSI = MotorControl::actualWheelBaseDistanceSI;
236
}
237

    
238
void Odometry::updateDistance() {
239

    
240
  // Get the current increments of the QEI
241
  MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference);
242
//
243
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);
244
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);
245

    
246
  // Get the driven distance for each wheel
247
  MotorControl::updateDistance(this->incrementDifference, this->distance);
248

    
249
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);
250
}