3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
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, ®, 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