Statistics
| Branch: | Tag: | Revision:

amiro-os / include / amiro / Odometry.h @ f3972840

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_ */