3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
Diff: omuni.cpp
- Revision:
- 0:3bd8aecadafc
diff -r 000000000000 -r 3bd8aecadafc omuni.cpp --- /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; +}