File content as of revision 5:17ede03f1fa5:

#ifndef MY_ODOMETER_H
#define MY_ODOMETER_H

#include "JY901.h"
#include "define.h"

class CountWheel
{
    public:
    CountWheel(Timer *t)
    {
        _t = t;
        _t -> start();
    }
    float getRadian(float frequency)
    {
        last_time = current_time;
        current_time = _t -> read();
        float dt = current_time - last_time;
        float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
        return delta_rad;
    }
    
    private:
    Timer *_t;
    float last_time;
    float current_time;
};


Timer tmp_t;
CountWheel counter[4] = {CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t)};

class MyOdometer
{
public:
    float x;
    float y;
    float theta;
    
    float vel_x;
    float vel_y;
    float vel_theta;
    
    float wheel_frequency[4];

    MyOdometer(JY901 *jy901)
    {
        x = 0;
        y = 0;
        theta = 0;
        vel_x = 0;
        vel_y = 0;
        vel_theta = 0;
        jy901_ = jy901;
        vel_timer.start();
    }
    void update()
    {
        theta = jy901_ -> calculateAngleOnlyGyro() * DEG_TO_RAD;
        //theta = jy901_ -> getZaxisAngle() * DEG_TO_RAD;
        for(int i = 0; i < MOTOR_NUM; i++)
        {
            wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]);
        }
        float dx = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
        float dy = ( wheel_rad[0] - wheel_rad[1] - wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
        
        x += dx * cos(theta) + dy * sin(-theta);
        y += dy * cos(theta) + dx * sin(theta);
        
        vel_x = (-wheel_frequency[0] + wheel_frequency[1] + wheel_frequency[2] - wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
                    
        vel_y = (-wheel_frequency[0] - wheel_frequency[1] + wheel_frequency[2] + wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
        
        float current_time = vel_timer.read();
        float dt = (current_time - last_time);
        vel_theta = (theta - last_theta) / dt;
        last_theta = theta; 
        last_time = current_time;
    }
    
private:
    JY901 *jy901_;
    Timer vel_timer;
    float wheel_rad[MOTOR_NUM];
    float last_time;
    float last_theta;
}; 

#endif