erasinator-2018/Odometry.h at master · Pitt-RAS/erasinator-2018 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
/**
 * Odometry Code
 * Xinke Chen
 */
#ifndef ODOMETRY_H_
#define ODOMETRY_H_

#include <Encoder.h>
#include "Buffer.h"

#define RADS_DEGREE 180/M_PI

class Odometry {
public:
    Odometry();
    double getHeading();
    double getHeadingDegrees();
    double getDistanceTraveled();
    double getVelocity();
    double getVelocity(double* wheel_velocities);
    void update();
private:
    Encoder encoder_a_;
    Encoder encoder_b_;
    Buffer velocity_buffer_a_;
    Buffer velocity_buffer_b_;
    void calculateDistanceTotal();
    void calculateVelocityInstantaneous(long delta_time, long delta_a, long delta_b);
    void calculateHeadingInstantaneous(long new_count_a, long new_count_b);
    double theta;
    double distance;
    double angular_velocity_a;
    double angular_velocity_b;
    long previous_time;
    long last_motor_count_a;
    long last_motor_count_b;
    // Constants
    static const uint8_t ENCODE_A_1;
    static const uint8_t ENCODE_A_2;
    static const uint8_t ENCODE_B_1;
    static const uint8_t ENCODE_B_2;
    static const double GEAR_RATIO;
    static const int ENCODER_RESOLUTION;
    static const double WHEEL_RADIUS;
    static const double WHEEL_CIRCUMFERENCE; // in millimeters
    static const int COUNTS_PER_REVOLUTION;
    static const double WHEEL_TRACK;
};
#endif