Statistics
| Branch: | Tag: | Revision:

amiro-os / components / Odometry.cpp @ 58fe0e0b

History | View | Annotate | Download (7.32 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

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

    
17

    
18
Odometry::Odometry(MotorIncrements* mi)
19
    : BaseStaticThread<512>(),
20
      motorIncrements(mi),
21
      eventSource(),
22
      period(50),
23
      incrementsPerRevolution(incrementsPerRevolution),
24
      updatesPerMinute(constants::secondsPerMinute * constants::millisecondsPerSecond / this->period),
25
      wheelCircumference(wheelCircumferenceSI),
26
      wheelBaseDistanceSI(wheelBaseDistanceSI) {
27

    
28

    
29
//  this-> = constants::secondsPerMinute * constants::millisecondsPerSecond / this->period;
30
//  this->wheelCircumference = constants::wheelCircumferenceSI;
31
//  this->wheelBaseDistanceSI = constants::wheelBaseDistanceSI;
32

    
33
  this->distance[LEFT_WHEEL] = 0.0f;
34
  this->distance[RIGHT_WHEEL] = 0.0f;
35
  this->increment[LEFT_WHEEL] = 0;
36
  this->increment[RIGHT_WHEEL] = 0;
37
  this->incrementDifference[LEFT_WHEEL] = 0.0f;
38
  this->incrementDifference[RIGHT_WHEEL] = 0.0f;
39
  this->distance[LEFT_WHEEL] = 0.0f;
40
  this->distance[RIGHT_WHEEL] = 0.0f;
41

    
42
  this->wheelError[LEFT_WHEEL] = wheelErrorSI[LEFT_WHEEL];
43
  this->wheelError[RIGHT_WHEEL] = wheelErrorSI[RIGHT_WHEEL];
44

    
45
  this->resetPosition(); // Init position
46

    
47
  this->resetError(); // Init error Cp
48

    
49
}
50

    
51
types::position Odometry::getPosition() {
52
  types::position robotPosition;
53
  const int32_t piScaled = int32_t(2 * M_PI * 1e6);
54
  chSysLock();
55
    // Conversion from standard unit to ยต unit
56
    robotPosition.x = this->pX * 1e6;
57
    robotPosition.y = this->pY * 1e6;
58
    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]
59
  chSysUnlock();
60
//     chprintf((BaseSequentialStream*) &SD1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);
61
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
62
//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
63
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
64
  return robotPosition;
65
}
66

    
67
void Odometry::setPosition(float pX, float pY, float pPhi) {
68
  chSysLock();
69
    this->pX = pX;
70
    this->pY = pY;
71
    this->pPhi = pPhi;
72
  chSysUnlock();
73
}
74

    
75
void Odometry::resetPosition() {
76
  this->setPosition(0.0f,0.0f,0.0f);
77
}
78

    
79
void Odometry::setError(float* Cp3x3) {
80
  chSysLock();
81
//   float** test;
82
    Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
83
//     Matrix::copy<float>(Cp3x3,3,3, test,3,3);
84
  chSysUnlock();
85
}
86

    
87
void Odometry::resetError() {
88
//   float zeroMatrix[9] = {};
89
//   this->setError(&(zeroMatrix[0]));
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 change after an calibration
105
    this->updateWheelBaseDistance();
106

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

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

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

    
118
    chThdSleepUntil(time);
119
  }
120

    
121
  return true;
122
}
123

    
124
void Odometry::updateOdometry() {
125

    
126
  // Get the temporary position and error
127
  float Cp3x3[9];
128
  chSysLock();
129
    float pX = this->pX;
130
    float pY = this->pY;
131
    float pPhi = this->pPhi;
132
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
133
  chSysUnlock();
134

    
135
  ////////////////
136
  // Temporary calculations
137
  ////////////////
138

    
139
  // TMP: Rotated angular
140
  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
141

    
142
  // TMP: Moved distance
143
  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
144

    
145
  // TMP: Argument for the trigonometric functions
146
  float trigArg = pPhi + dPhi / 2.0f;
147

    
148
  // TMP: Trigonometric functions
149
  float cosArg = cos(trigArg);
150
  float sinArg = sin(trigArg);
151

    
152
  // TMP: Delta distance
153
  float dPX = dDistance * cosArg;
154
  float dPY = dDistance * sinArg;
155

    
156
  ////////////////
157
  // Position Update
158
  ////////////////
159

    
160
  // Update distance
161
  pX += dPX;
162
  pY += dPY;
163
  pPhi += dPhi;
164

    
165
  ////////////////
166
  // Temporary error calculations
167
  ////////////////
168

    
169
  // position propagation error (3x3 matrix)
170
  float Fp3x3[9]  = {1.0f, 0.0f, -dPY,
171
                     0.0f, 1.0f,  dPX,
172
                     0.0f, 0.0f, 1.0f};
173
  // steering error (2x2 matrix)
174
  float Cs2x2[4] = {abs(this->distance[RIGHT_WHEEL])*wheelError[RIGHT_WHEEL],0.0f,
175
                    0.0f, abs(this->distance[LEFT_WHEEL])*wheelError[LEFT_WHEEL]};
176
  // steering propagation error (3x2 matrix)
177
  float Fs3x2[6] = {(cosArg+dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f, (sinArg+dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f,
178
                 (sinArg-dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f, (cosArg-dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f,
179
                 -1.0f/this->wheelBaseDistanceSI                         , 1.0f/this->wheelBaseDistanceSI};
180

    
181
  ////////////////
182
  // Error calculations tmpCp = Fp*Cp*~Fp
183
  ////////////////
184
  // New position error
185
  float tmpCp3x3[9] = {0.0f};
186
  float tmpFpCp3x3[9] = {0.0f};
187
  // tmpFpCp = Fp*Cp
188
  Matrix::XdotY<float>(&(Fp3x3[0]),3,3,&(Cp3x3[0]),3,3,&(tmpFpCp3x3[0]),3,3);
189
  // tmpCp = tmpFpCp*~Fp
190
  Matrix::XdotYtrans<float>(&(tmpFpCp3x3[0]),3,3,&(Fp3x3[0]),3,3,&(tmpCp3x3[0]),3,3);
191

    
192
  ////////////////
193
  // Error calculations tmpCs = Fs*Cs*~Fs
194
  ////////////////
195
  // New steering error
196
  float tmpCs3x3[9] = {0.0f};
197
  float tmpFsCs3x2[6] = {0.0f};
198
  // tmpFsCs = Fs*Cs
199
  Matrix::XdotY<float>(&(Fs3x2[0]),3,2,&(Cs2x2[0]),2,2,&(tmpFsCs3x2[0]),3,2);
200
  // tmpCs = tmpFsCs*~Fs
201
  Matrix::XdotYtrans<float>(&(tmpFsCs3x2[0]),3,2,&(Fs3x2[0]),3,2,&(tmpCs3x3[0]),3,3);
202

    
203
  ////////////////
204
  // Error calculations Cp = Fp*Cp*~Fp + Fs*Cs*~Fs
205
  ////////////////
206
  Matrix::XplusY<float>(tmpCp3x3,3,3,tmpCs3x3,3,3,Cp3x3,3,3);
207

    
208
  ////////////////
209
  // Write back
210
  ////////////////
211

    
212
  // Write back
213
  this->setPosition(pX,pY,pPhi);
214
  chSysLock();
215
    Matrix::copy<float>(Cp3x3,3,3,this->Cp3x3,3,3);
216
  chSysUnlock();
217

    
218
}
219

    
220
void Odometry::updateWheelBaseDistance() {
221
  this->wheelBaseDistanceSI = MotorControl::actualWheelBaseDistanceSI;
222
}
223

    
224
void Odometry::updateDistance() {
225

    
226
  // Get the current increments of the QEI
227
  MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference);
228
//
229
//  chprintf((BaseSequentialStream*) &SD1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);
230
//  chprintf((BaseSequentialStream*) &SD1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);
231

    
232
  // Get the driven distance for each wheel
233
  MotorControl::updateDistance(this->incrementDifference, this->distance);
234

    
235
//  chprintf((BaseSequentialStream*) &SD1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);
236
}