Revision 0:3bd8aecadafc, committed 2017-09-28

Comitter:

sawai

Date:

Thu Sep 28 10:50:37 2017 +0000

Commit message:

?????????xy?????; ???????????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20_i2c/L3GD20_i2c.cpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "L3GD20_i2c.hpp"
+
+l3gd20::l3gd20(I2C *_i, bool SA0):ADDR_WHO_AM_I(0x0f), ADDR_CTRL_REG1(0x20), ADDR_CTRL_REG2(0x21), ADDR_CTRL_REG3(0x22)
+                            , ADDR_CTRL_REG4(0x23), ADDR_CTRL_REG5(0x24), ADDR_OUT_TEMP(0x26), ADDR_STATUS_REG(0x27)
+                            , ADDR_OUT_X_L(0x28), ADDR_OUT_X_H(0x29), ADDR_OUT_Y_L(0x2A), ADDR_OUT_Y_H(0x2B), ADDR_OUT_Z_L(0x2C), ADDR_OUT_Z_H(0x2D)
+{
+    pre_time_ms = 0;
+    i = _i;
+    if(SA0) addr = 0xD6;
+    else    addr = 0xD4;
+    decom = 0.0875;
+    write_reg(ADDR_CTRL_REG1, 0x0F);
+    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
+}
+
+char l3gd20::read_reg(char reg)
+{
+    char recv;
+    i->write(addr, &reg, 1, true);
+    i->read(addr | 0x01, &recv, 1);
+    return recv;
+}
+
+void l3gd20::write_reg(char reg, char data)
+{
+    char send[] = {reg, data};
+    i->write(addr, send, 2);
+}
+
+bool l3gd20::who_am_i()
+{
+    return read_reg(ADDR_WHO_AM_I) == 0xD4;
+}
+
+void l3gd20::set_range(int dps)
+{
+    switch(dps)
+    {
+        case 250:
+            decom = 0.00875;
+            write_reg(ADDR_CTRL_REG4, 0b00000000);
+            break;
+            
+        case 500:
+            decom = 0.0175;
+            write_reg(ADDR_CTRL_REG4, 0b00010000);
+            break;
+            
+        case 2000:
+            decom = 0.07;
+            write_reg(ADDR_CTRL_REG4, 0b00100000);
+            break;
+            
+    }
+}
+
+int16_t l3gd20::get_raw_omega(char se)
+{
+    char addrRegH, addrRegL;
+    switch(se)
+    {
+        case 'x': case 'X':
+            addrRegH = ADDR_OUT_X_H;
+            addrRegL = ADDR_OUT_X_L;
+            break;
+            
+        case 'y': case 'Y':
+            addrRegH = ADDR_OUT_Y_H;
+            addrRegL = ADDR_OUT_Y_L;
+            break;
+            
+        case 'z':case 'Z':
+            addrRegH = ADDR_OUT_Z_H;
+            addrRegL = ADDR_OUT_Z_L;
+            break;
+    }
+    int16_t rawOmegaH = read_reg(addrRegH);
+    int16_t rawOmegaL = read_reg(addrRegL);
+    
+    return  (rawOmegaH << 8) | rawOmegaL;
+}
+
+void l3gd20::apply_offset()
+{
+    int num = 100;
+    double accum[3] = {0};
+    for(int i = 0; i < num; i++)
+    {
+        accum[0] += get_raw_omega('X');
+        accum[1] += get_raw_omega('Y');
+        accum[2] += get_raw_omega('Z');
+        wait_ms(1);
+    }
+    for(int i = 0; i < 3; i++)
+    {
+        omegaOffset[i] = accum[i] * decom / (double)num;
+    }
+}
+
+void l3gd20::start()
+{
+    t.start();
+    stpFlg = false;
+}
+
+void l3gd20::reset()
+{
+    t.reset();
+    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
+}
+
+void l3gd20::stop()
+{
+    t.stop();
+    t.reset();
+    stpFlg = true;
+}
+
+double l3gd20::trapezoid_integr(double data, double ex_data, double period){
+    return (data + ex_data) * period / 2.0;
+}
+
+void l3gd20::renew_angle()
+{
+    static double preOmega[3] = {0};
+    
+    if(stpFlg) return;
+    
+    double omega[] = 
+    {
+        get_raw_omega('X') * decom - omegaOffset[0],
+        get_raw_omega('Y') * decom - omegaOffset[1],
+        get_raw_omega('Z') * decom - omegaOffset[2]
+    };
+    
+    int dt_ms = t.read_ms();
+//    t.reset();
+    
+    for(int i = 0; i < 3; i++)
+    {
+        angleDeg[i] += trapezoid_integr(omega[i], preOmega[i], (dt_ms - pre_time_ms) / 1000.0);
+        preOmega[i] = omega[i];
+    }
+    pre_time_ms = dt_ms;
+    
+    if(t.read() > 1800)
+    {
+        t.reset();
+        pre_time_ms = 0;
+    }
+}
+
+void l3gd20::get_angle_deg(double *a)
+{
+    for(int i = 0; i < 3; i++)
+    {
+        a[i] = angleDeg[i];
+    }
+}
+
+void l3gd20::get_angle_rad(double *a)
+{
+    for(int i = 0; i < 3; i++)
+    {
+        a[i] = angleDeg[i] / 180.0 * 3.141593;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20_i2c/L3GD20_i2c.hpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,47 @@
+#ifndef __L3GD20_I2C__
+#define __L3GD20_I2C__
+
+#include "mbed.h"
+
+class l3gd20
+{
+    I2C *i;
+    Timer t;
+    long pre_time_ms;
+    char addr;
+    const char ADDR_WHO_AM_I;
+    const char ADDR_CTRL_REG1;
+    const char ADDR_CTRL_REG2;
+    const char ADDR_CTRL_REG3;
+    const char ADDR_CTRL_REG4;
+    const char ADDR_CTRL_REG5;
+    const char ADDR_OUT_TEMP;
+    const char ADDR_STATUS_REG;
+    const char ADDR_OUT_X_L;
+    const char ADDR_OUT_X_H;
+    const char ADDR_OUT_Y_L;
+    const char ADDR_OUT_Y_H;
+    const char ADDR_OUT_Z_L;
+    const char ADDR_OUT_Z_H;
+    double decom;
+    double angleDeg[3];
+    double omegaOffset[3];
+    bool stpFlg;
+public:
+    l3gd20(I2C *_i, bool SA0);
+    char read_reg(char reg);
+    void write_reg(char reg, char data);
+    bool who_am_i();
+    void set_range(int dps);
+    int16_t get_raw_omega(char se);
+    void apply_offset();
+    void start();
+    void reset();
+    void stop();
+    double trapezoid_integr(double data, double ex_data, double period);
+    void renew_angle();
+    void get_angle_deg(double *a);
+    void get_angle_rad(double *a);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omuni.cpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,181 @@
+#include "mbed.h"
+#include "omuni.hpp"
+#include "speed_control.hpp"
+#include "L3GD20_i2c.hpp"
+
+Omuni::Omuni(I2C *i, TIM_TypeDef *timer_type0, TIM_TypeDef *timer_type1, TIM_TypeDef *timer_type2, size_t pulse_per_revol, double _cy_ms, const int a[3], double _R, double _d)
+{
+    i2c = i;
+    gyro = new l3gd20(i2c, false);
+    gyro->set_range(2000);
+    gyro->apply_offset();
+    gyro->start();
+    o[0] = new speed_control(timer_type0, pulse_per_revol, _cy_ms);
+    o[1] = new speed_control(timer_type1, pulse_per_revol, _cy_ms);
+    o[2] = new speed_control(timer_type2, pulse_per_revol, _cy_ms);
+    o[0]->change_direction(false);
+    o[1]->change_direction(false);
+    o[2]->change_direction(false);
+    R = _R; d = _d;
+    vx = 0.0; vy = 0.0; omega = 0.0;
+    for(int i = 0; i < 3; i++)
+    {
+        addr[i] = a[i];
+        v[i] = 0.0;
+    }
+    time.start(); t.start();
+    f = false;
+    crevx = false;  crevy = false;
+}
+void Omuni::set_pid(int n, double _kp, double _ki, double _kd)
+{
+    if(n >= 0 && n <= 2)
+    {
+        o[n]->set_pid(_kp, _ki, _kd);
+    }
+}
+
+void Omuni::change_motor_direction(int n, bool _rev)
+{
+    if(n >= 0 && n <= 2)
+    {
+        o[n]->change_direction(_rev);
+    }
+}
+
+void Omuni::change_coordinates_direction(bool _crevx, bool _crevy)
+{
+    crevx = _crevx;
+    crevy = _crevy;
+}
+
+void Omuni::set_speed(double _vx, double _vy)
+{
+    tvx = _vx; tvy = _vy;
+    if(crevx)       tvx *= -1;
+    if(crevy)       tvy *= -1;
+}
+
+void Omuni::set_speed(double _vx, double _vy, double _omega)
+{
+    set_speed(_vx, _vy);
+    omega = _omega;
+}
+
+void Omuni::set_speed(double _vx, double _vy, double _omega, bool _f)
+{
+    set_speed(_vx, _vy, _omega);
+    f = _f;
+    if(!f)  gyro->reset();
+}
+
+void Omuni::renew_theta()
+{
+    double theta_temp[3];
+    gyro->renew_angle();
+    gyro->get_angle_rad(theta_temp);
+    theta = theta_temp[2] * -1;
+    /*
+    static double now_time, pre_time;
+    now_time = time.read();
+    
+    if(now_time - pre_time < 0.1f)
+    {
+        if(f == true)
+        {
+            theta += (omega * (now_time - pre_time)) * THETA_ADDJUST;
+        }
+        else
+        {
+            theta = 0.0f;
+        }
+    }
+    if(now_time > 1800)
+    {
+        time.reset();
+        pre_time = 0.0f;
+    }
+    pre_time = now_time;
+    */
+}
+
+bool Omuni::renew_speed()
+{
+    const double a = 10;
+    static double now_time, pre_time;
+    now_time = time.read();
+    if(now_time - pre_time > 0.01)
+    {
+        pre_time = now_time;
+        
+        if(tvx - vx <= a * 0.01 && tvx - vx >= -1 * a * 0.01 && tvy - vy <= a * 0.01 && tvy - vy >= -1 * a * 0.01)
+        {
+            vx = tvx; vy = tvy;
+        }
+        else
+        {
+            double x, y;
+            x = tvx - vx;
+            y = tvy - vy;
+            vx += a * 0.01 * x / sqrt(x * x + y * y);
+            vy += a * 0.01 * y / sqrt(x * x + y * y);
+        }
+        return true;
+    }
+    return false;
+}
+
+void Omuni::_set_speed()
+{
+    //if(renew_speed())
+    {
+        double _vx, _vy;
+        _vx =      tvx * cos(theta) + tvy * sin(theta);
+        _vy = -1 * tvx * sin(theta) + tvy * cos(theta);
+        v[0] = (-1.0 * _vx              ) / (3.1416 * d);
+        v[1] = ( 0.5 * _vx - 0.866 * _vy) / (3.1416 * d);
+        v[2] = ( 0.5 * _vx + 0.866 * _vy) / (3.1416 * d);
+        v[0] += (R * omega) / (3.1416 * d);
+        v[1] += (R * omega) / (3.1416 * d);
+        v[2] += (R * omega) / (3.1416 * d);
+        
+        
+        for(int i = 0; i < 3; i++)
+        {
+            o[i]->set_speed(v[i]);
+        }
+    }
+}
+
+void Omuni::drive()
+{
+    renew_theta();
+    
+    _set_speed();
+    
+    for(int i = 0; i < 3; i++)
+    {
+        char duty = o[i]->get_duty();
+        i2c->write(addr[i], &duty, 1);
+    }
+}
+
+void Omuni::reset_theta()
+{
+    theta = 0.0;
+}
+
+double Omuni::get_theta()
+{
+    return theta;
+}
+
+double Omuni::get_vx()
+{
+    return vx;
+}
+
+double Omuni::get_vy()
+{
+    return vy;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omuni.hpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,41 @@
+#ifndef _omuni_
+#define _omuni_
+
+#include "speed_control.hpp"
+#include "L3GD20_i2c.hpp"
+
+#define THETA_ADDJUST   0.69f
+
+class Omuni
+{
+private:
+    I2C *i2c;
+    l3gd20 *gyro;
+    speed_control *o[3];
+    Timer time, t;
+    double tvx, tvy, tomega; 
+    double vx, vy, omega, theta;
+    double R, d;
+    int addr[3];
+    double v[3];
+    bool f;
+    bool crevx, crevy;
+public:
+    Omuni(I2C *i, TIM_TypeDef *timer_type0, TIM_TypeDef *timer_type1, TIM_TypeDef *timer_type2, size_t pulse_per_revol, double _cy_ms, const int a[3], double _R, double _d);
+    void set_pid(int n, double _kp, double _ki, double _kd);
+    void change_motor_direction(int n, bool _rev);
+    void change_coordinates_direction(bool _crevx, bool _crevy);
+    void set_speed(double vx, double vy);
+    void set_speed(double vx, double vy, double omega);
+    void set_speed(double _vx, double _vy, double _omega, bool _f);
+    void renew_theta();
+    bool renew_speed();
+    void _set_speed();
+    void drive();
+    void reset_theta();
+    double get_theta();
+    double get_vx();
+    double get_vy();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_control/nucleo_rotary_encoder.lib	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/inst/code/nucleo_rotary_encoder/#684e1604e5ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_control/speed_control.cpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "speed_control.hpp"
+#include "rotary_encoder_ab_phase.hpp"
+
+speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms)
+{
+    e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol);
+    pulse_per_revol = _pulse_per_revol;
+    cy_ms = _cy_ms;
+    f1 = f2 = 0;
+    target_value = 0.0f;
+    
+    rev = false;
+    
+    e->start();
+    t.start();
+}
+
+void speed_control::change_direction(bool _rev)
+{
+    rev = _rev;
+}
+
+void speed_control::set_speed(float _target_value)
+{
+    target_value = _target_value;
+}
+
+void speed_control::set_pid(float _kp, float _ki, float _kd)
+{
+    kp = _kp; ki = _ki; kd = _kd;
+    f1 = kp + ki / 2.0f + kd;
+    f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd;
+    
+    e->reset();
+    t.reset();
+    time_counter = 0;
+    pre_counts = 0;
+}
+
+bool speed_control::check_get_speed()
+{
+    if(t.read_ms() > time_counter)
+    {
+        time_counter += cy_ms;
+        
+        for(int i = 9; i > 0; i--)
+        {
+            e_value[i] = e_value[i-1];
+        }
+        int32_t ec = e->get_counts();
+        e_value[0] = ec - pre_counts;
+        if(ec > 20000 || ec < -20000)
+        {
+            e->reset();
+            pre_counts = pre_counts - ec;
+        }
+        else
+        {
+            pre_counts = ec;
+        }
+        int32_t esum = 0;
+        for(int i = 0; i < 10; i++)
+        {
+            esum += e_value[i];
+        }
+        sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000;
+        
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+char speed_control::get_duty()
+{
+    if(t.read() > 1800)
+    {
+        t.reset();
+        time_counter = 0;
+    }
+    if(check_get_speed())
+    {
+        diff[0] = sensor_value - target_value;
+        dm = f1 * diff[0] + j;
+        m = m + dm;
+        
+        int m_int = (int)m * -1;
+        if(m_int <= -126)   m_int = -126;
+        if(m_int >=  126)   m_int =  126;
+        duty = m_int;
+        
+        j = f2 * diff[0] + kd * diff[1];
+        diff[1] = diff[0];
+    }
+    
+    if(rev)duty *= -1;
+    
+    return duty;
+}
+
+float speed_control::get_speed()
+{
+    return sensor_value;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_control/speed_control.hpp	Thu Sep 28 10:50:37 2017 +0000
@@ -0,0 +1,31 @@
+#ifndef __SPEED_CONTROL__
+#define __SPEED_CONTROL__
+
+#include "mbed.h"
+#include "rotary_encoder_ab_phase.hpp"
+
+class speed_control
+{
+private:
+    rotary_encoder_ab_phase *e;
+    Timer t;
+    float kp, ki, kd;
+    float f1, f2, diff[2], m, dm, j;
+    float pulse_per_revol, cy_ms;
+    float target_value, sensor_value;
+    unsigned long time_counter;
+    int32_t e_value[10], pre_counts;
+    char duty;
+    bool rev;
+    
+public:
+    speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms);
+    void change_direction(bool _rev);
+    void set_speed(float _target_value);
+    void set_pid(float _kp, float _ki, float _kd);
+    bool check_get_speed();
+    char get_duty();
+    float get_speed();
+};
+
+#endif
\ No newline at end of file