Ryosuke Matsushima / Encoder

Dependents:   TUKUBAMotorDriver

Encoder.cpp

Committer:
ryosukenanoda
Date:
2021-01-25
Revision:
2:36572948b0d4
Parent:
1:c44f4aa7a45d
Child:
3:cf7ae37c6930

File content as of revision 2:36572948b0d4:

#include "mbed.h"
#include "Encoder.h"

Encoder::Encoder(PinName pin_A, PinName pin_B): _pin_A(pin_A), _pin_B(pin_B) {
    max_time = 2^30; //mbet can count only 30 minits --- https://os.mbed.com/handbook/Timer
    dt_to_raito = 2 * PI / (RESOLUTION_COUNT * GEAR_RAITO); // dt_to_raito / dt = raito
    timer.start();
    _pin_A.rise(callback(this, &Encoder::did_rise));
};

float Encoder::get_raito() {
    return raito;
};

void Encoder::did_rise() {
    int now_time = timer.read_us();
    if( false ) { //TODO: reset timer if time_maxmum
        last_time = 0;
        timer.reset();
        return;
    }
    for (int i = 0; i < 3; i++) {
        if (!_pin_A) {
            wait_us(10);
            return;
        }
    }
    int dt = now_time - last_time;
    stop_checker.attach(callback(this, &Encoder::check_stop), 0.01);
    update_raito(dt);
    last_time = now_time;
};

void Encoder::update_raito(int dt) {
    float t = dt;
    t /= pow(10.0, 6.0);
    raito = dt_to_raito / t * direction();
};

float Encoder::direction() {
    if (_pin_B) {
        return -1;
    } else {
        return 1;
    }
};

void Encoder::check_stop() {
    int now_time = timer.read_us();
    if ( (now_time - last_time) > 10000 ) raito = 0.0;
};