amiro-os / include / amiro / Odometry.h @ b4885314
History | View | Annotate | Download (2.52 KB)
1 |
#ifndef AMIRO_ODOMETRY_H_
|
---|---|
2 |
#define AMIRO_ODOMETRY_H_
|
3 |
|
4 |
#include <amiro/MotorControl.h> |
5 |
#include <amiro/gyro/l3g4200d.hpp> |
6 |
|
7 |
#include <Types.h> // types::position |
8 |
|
9 |
namespace amiro { |
10 |
|
11 |
class Odometry : public chibios_rt::BaseStaticThread<512> {
|
12 |
public:
|
13 |
/**
|
14 |
* Constructor
|
15 |
*
|
16 |
* @param mi object for retrieving the motor increments of the qei
|
17 |
* @param gyro object for retrieving the gyroscope data
|
18 |
* @param mc object for retrieving calibration parameters
|
19 |
*/
|
20 |
Odometry(MotorIncrements* mi, L3G4200D* gyroscope); |
21 |
|
22 |
/**
|
23 |
* Set the position of the roboter
|
24 |
*
|
25 |
* @param pX Position x in meter
|
26 |
* @param pY Position y in meter
|
27 |
* @param pPhi Orientation phi in radiant
|
28 |
*/
|
29 |
void setPosition(float pX, float pY, float pPhi); |
30 |
|
31 |
/**
|
32 |
* Reset the position to [0,0,0]
|
33 |
*/
|
34 |
void resetPosition();
|
35 |
|
36 |
/**
|
37 |
* Set the position of the roboter
|
38 |
*
|
39 |
* @param *Cp
|
40 |
*/
|
41 |
void setError(float* Cp); |
42 |
|
43 |
/**
|
44 |
* Reset the error to [0,0,0;0,0,0;0,0,0]
|
45 |
*/
|
46 |
void resetError();
|
47 |
|
48 |
/**
|
49 |
* Updates the wheelBaseDistance variable,
|
50 |
* because it may change after an calibration.
|
51 |
*/
|
52 |
void updateWheelBaseDistance();
|
53 |
|
54 |
/**
|
55 |
* Get the current position
|
56 |
* @return position
|
57 |
*/
|
58 |
types::position getPosition(); |
59 |
|
60 |
chibios_rt::EvtSource* getEventSource(); |
61 |
|
62 |
protected:
|
63 |
virtual msg_t main(void);
|
64 |
|
65 |
private:
|
66 |
|
67 |
/**
|
68 |
* Calculate the actual distance of both wheels
|
69 |
*/
|
70 |
void updateDistance();
|
71 |
|
72 |
/**
|
73 |
* This is th PID controller for controling
|
74 |
* the speed. It gets the new duty cicle for the pwm
|
75 |
* and saves it to this->pwmPercentage to reach the
|
76 |
* target speed this->targetSpeed
|
77 |
*/
|
78 |
void updateOdometry();
|
79 |
|
80 |
MotorIncrements* motorIncrements; // QEI driver
|
81 |
L3G4200D* gyro; // Gyroscope driver
|
82 |
chibios_rt::EvtSource eventSource; |
83 |
const unsigned int period; |
84 |
int incrementsPerRevolution;
|
85 |
int updatesPerMinute;
|
86 |
float wheelCircumference; // wheel circumference in meter |
87 |
float wheelBaseDistanceSI; // Base width in meter |
88 |
float distance[2]; // Moved distance per wheel in meter left:0 right:1 |
89 |
float wheelError[2]; // error for left:0 and right:1 wheel |
90 |
float pX, pY; // Position in meter |
91 |
float pPhi; // Orientation in Rad |
92 |
float Cp3x3[9]; // Covariance (position error) (3x3 matrix) |
93 |
int32_t increment[2]; // Absolute number of difference since last update |
94 |
float incrementDifference[2]; // Difference between old and current absolute increments |
95 |
}; |
96 |
|
97 |
} |
98 |
|
99 |
#endif /* AMIRO_ODOMETRY_ */ |