amiro-os / include / amiro / Odometry.h @ 19a69797
History | View | Annotate | Download (2.52 KB)
1 | 58fe0e0b | Thomas Schöpping | #ifndef AMIRO_ODOMETRY_H_
|
---|---|---|---|
2 | #define AMIRO_ODOMETRY_H_
|
||
3 | |||
4 | #include <amiro/MotorControl.h> |
||
5 | b4885314 | Thomas Schöpping | #include <amiro/gyro/l3g4200d.hpp> |
6 | 58fe0e0b | Thomas Schöpping | |
7 | #include <Types.h> // types::position |
||
8 | |||
9 | namespace amiro { |
||
10 | |||
11 | f336542d | Timo Korthals | class Odometry : public chibios_rt::BaseStaticThread<512> {
|
12 | 58fe0e0b | Thomas Schöpping | public:
|
13 | /**
|
||
14 | * Constructor
|
||
15 | *
|
||
16 | * @param mi object for retrieving the motor increments of the qei
|
||
17 | b4885314 | Thomas Schöpping | * @param gyro object for retrieving the gyroscope data
|
18 | * @param mc object for retrieving calibration parameters
|
||
19 | 58fe0e0b | Thomas Schöpping | */
|
20 | b4885314 | Thomas Schöpping | Odometry(MotorIncrements* mi, L3G4200D* gyroscope); |
21 | 58fe0e0b | Thomas Schöpping | |
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 | b4885314 | Thomas Schöpping | MotorIncrements* motorIncrements; // QEI driver
|
81 | L3G4200D* gyro; // Gyroscope driver
|
||
82 | 58fe0e0b | Thomas Schöpping | 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_ */ |