3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
omuni.cpp@0:3bd8aecadafc, 2017-09-28 (annotated)
- Committer:
- sawai
- Date:
- Thu Sep 28 10:50:37 2017 +0000
- Revision:
- 0:3bd8aecadafc
?????????xy?????; ???????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sawai | 0:3bd8aecadafc | 1 | #include "mbed.h" |
sawai | 0:3bd8aecadafc | 2 | #include "omuni.hpp" |
sawai | 0:3bd8aecadafc | 3 | #include "speed_control.hpp" |
sawai | 0:3bd8aecadafc | 4 | #include "L3GD20_i2c.hpp" |
sawai | 0:3bd8aecadafc | 5 | |
sawai | 0:3bd8aecadafc | 6 | 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) |
sawai | 0:3bd8aecadafc | 7 | { |
sawai | 0:3bd8aecadafc | 8 | i2c = i; |
sawai | 0:3bd8aecadafc | 9 | gyro = new l3gd20(i2c, false); |
sawai | 0:3bd8aecadafc | 10 | gyro->set_range(2000); |
sawai | 0:3bd8aecadafc | 11 | gyro->apply_offset(); |
sawai | 0:3bd8aecadafc | 12 | gyro->start(); |
sawai | 0:3bd8aecadafc | 13 | o[0] = new speed_control(timer_type0, pulse_per_revol, _cy_ms); |
sawai | 0:3bd8aecadafc | 14 | o[1] = new speed_control(timer_type1, pulse_per_revol, _cy_ms); |
sawai | 0:3bd8aecadafc | 15 | o[2] = new speed_control(timer_type2, pulse_per_revol, _cy_ms); |
sawai | 0:3bd8aecadafc | 16 | o[0]->change_direction(false); |
sawai | 0:3bd8aecadafc | 17 | o[1]->change_direction(false); |
sawai | 0:3bd8aecadafc | 18 | o[2]->change_direction(false); |
sawai | 0:3bd8aecadafc | 19 | R = _R; d = _d; |
sawai | 0:3bd8aecadafc | 20 | vx = 0.0; vy = 0.0; omega = 0.0; |
sawai | 0:3bd8aecadafc | 21 | for(int i = 0; i < 3; i++) |
sawai | 0:3bd8aecadafc | 22 | { |
sawai | 0:3bd8aecadafc | 23 | addr[i] = a[i]; |
sawai | 0:3bd8aecadafc | 24 | v[i] = 0.0; |
sawai | 0:3bd8aecadafc | 25 | } |
sawai | 0:3bd8aecadafc | 26 | time.start(); t.start(); |
sawai | 0:3bd8aecadafc | 27 | f = false; |
sawai | 0:3bd8aecadafc | 28 | crevx = false; crevy = false; |
sawai | 0:3bd8aecadafc | 29 | } |
sawai | 0:3bd8aecadafc | 30 | void Omuni::set_pid(int n, double _kp, double _ki, double _kd) |
sawai | 0:3bd8aecadafc | 31 | { |
sawai | 0:3bd8aecadafc | 32 | if(n >= 0 && n <= 2) |
sawai | 0:3bd8aecadafc | 33 | { |
sawai | 0:3bd8aecadafc | 34 | o[n]->set_pid(_kp, _ki, _kd); |
sawai | 0:3bd8aecadafc | 35 | } |
sawai | 0:3bd8aecadafc | 36 | } |
sawai | 0:3bd8aecadafc | 37 | |
sawai | 0:3bd8aecadafc | 38 | void Omuni::change_motor_direction(int n, bool _rev) |
sawai | 0:3bd8aecadafc | 39 | { |
sawai | 0:3bd8aecadafc | 40 | if(n >= 0 && n <= 2) |
sawai | 0:3bd8aecadafc | 41 | { |
sawai | 0:3bd8aecadafc | 42 | o[n]->change_direction(_rev); |
sawai | 0:3bd8aecadafc | 43 | } |
sawai | 0:3bd8aecadafc | 44 | } |
sawai | 0:3bd8aecadafc | 45 | |
sawai | 0:3bd8aecadafc | 46 | void Omuni::change_coordinates_direction(bool _crevx, bool _crevy) |
sawai | 0:3bd8aecadafc | 47 | { |
sawai | 0:3bd8aecadafc | 48 | crevx = _crevx; |
sawai | 0:3bd8aecadafc | 49 | crevy = _crevy; |
sawai | 0:3bd8aecadafc | 50 | } |
sawai | 0:3bd8aecadafc | 51 | |
sawai | 0:3bd8aecadafc | 52 | void Omuni::set_speed(double _vx, double _vy) |
sawai | 0:3bd8aecadafc | 53 | { |
sawai | 0:3bd8aecadafc | 54 | tvx = _vx; tvy = _vy; |
sawai | 0:3bd8aecadafc | 55 | if(crevx) tvx *= -1; |
sawai | 0:3bd8aecadafc | 56 | if(crevy) tvy *= -1; |
sawai | 0:3bd8aecadafc | 57 | } |
sawai | 0:3bd8aecadafc | 58 | |
sawai | 0:3bd8aecadafc | 59 | void Omuni::set_speed(double _vx, double _vy, double _omega) |
sawai | 0:3bd8aecadafc | 60 | { |
sawai | 0:3bd8aecadafc | 61 | set_speed(_vx, _vy); |
sawai | 0:3bd8aecadafc | 62 | omega = _omega; |
sawai | 0:3bd8aecadafc | 63 | } |
sawai | 0:3bd8aecadafc | 64 | |
sawai | 0:3bd8aecadafc | 65 | void Omuni::set_speed(double _vx, double _vy, double _omega, bool _f) |
sawai | 0:3bd8aecadafc | 66 | { |
sawai | 0:3bd8aecadafc | 67 | set_speed(_vx, _vy, _omega); |
sawai | 0:3bd8aecadafc | 68 | f = _f; |
sawai | 0:3bd8aecadafc | 69 | if(!f) gyro->reset(); |
sawai | 0:3bd8aecadafc | 70 | } |
sawai | 0:3bd8aecadafc | 71 | |
sawai | 0:3bd8aecadafc | 72 | void Omuni::renew_theta() |
sawai | 0:3bd8aecadafc | 73 | { |
sawai | 0:3bd8aecadafc | 74 | double theta_temp[3]; |
sawai | 0:3bd8aecadafc | 75 | gyro->renew_angle(); |
sawai | 0:3bd8aecadafc | 76 | gyro->get_angle_rad(theta_temp); |
sawai | 0:3bd8aecadafc | 77 | theta = theta_temp[2] * -1; |
sawai | 0:3bd8aecadafc | 78 | /* |
sawai | 0:3bd8aecadafc | 79 | static double now_time, pre_time; |
sawai | 0:3bd8aecadafc | 80 | now_time = time.read(); |
sawai | 0:3bd8aecadafc | 81 | |
sawai | 0:3bd8aecadafc | 82 | if(now_time - pre_time < 0.1f) |
sawai | 0:3bd8aecadafc | 83 | { |
sawai | 0:3bd8aecadafc | 84 | if(f == true) |
sawai | 0:3bd8aecadafc | 85 | { |
sawai | 0:3bd8aecadafc | 86 | theta += (omega * (now_time - pre_time)) * THETA_ADDJUST; |
sawai | 0:3bd8aecadafc | 87 | } |
sawai | 0:3bd8aecadafc | 88 | else |
sawai | 0:3bd8aecadafc | 89 | { |
sawai | 0:3bd8aecadafc | 90 | theta = 0.0f; |
sawai | 0:3bd8aecadafc | 91 | } |
sawai | 0:3bd8aecadafc | 92 | } |
sawai | 0:3bd8aecadafc | 93 | if(now_time > 1800) |
sawai | 0:3bd8aecadafc | 94 | { |
sawai | 0:3bd8aecadafc | 95 | time.reset(); |
sawai | 0:3bd8aecadafc | 96 | pre_time = 0.0f; |
sawai | 0:3bd8aecadafc | 97 | } |
sawai | 0:3bd8aecadafc | 98 | pre_time = now_time; |
sawai | 0:3bd8aecadafc | 99 | */ |
sawai | 0:3bd8aecadafc | 100 | } |
sawai | 0:3bd8aecadafc | 101 | |
sawai | 0:3bd8aecadafc | 102 | bool Omuni::renew_speed() |
sawai | 0:3bd8aecadafc | 103 | { |
sawai | 0:3bd8aecadafc | 104 | const double a = 10; |
sawai | 0:3bd8aecadafc | 105 | static double now_time, pre_time; |
sawai | 0:3bd8aecadafc | 106 | now_time = time.read(); |
sawai | 0:3bd8aecadafc | 107 | if(now_time - pre_time > 0.01) |
sawai | 0:3bd8aecadafc | 108 | { |
sawai | 0:3bd8aecadafc | 109 | pre_time = now_time; |
sawai | 0:3bd8aecadafc | 110 | |
sawai | 0:3bd8aecadafc | 111 | if(tvx - vx <= a * 0.01 && tvx - vx >= -1 * a * 0.01 && tvy - vy <= a * 0.01 && tvy - vy >= -1 * a * 0.01) |
sawai | 0:3bd8aecadafc | 112 | { |
sawai | 0:3bd8aecadafc | 113 | vx = tvx; vy = tvy; |
sawai | 0:3bd8aecadafc | 114 | } |
sawai | 0:3bd8aecadafc | 115 | else |
sawai | 0:3bd8aecadafc | 116 | { |
sawai | 0:3bd8aecadafc | 117 | double x, y; |
sawai | 0:3bd8aecadafc | 118 | x = tvx - vx; |
sawai | 0:3bd8aecadafc | 119 | y = tvy - vy; |
sawai | 0:3bd8aecadafc | 120 | vx += a * 0.01 * x / sqrt(x * x + y * y); |
sawai | 0:3bd8aecadafc | 121 | vy += a * 0.01 * y / sqrt(x * x + y * y); |
sawai | 0:3bd8aecadafc | 122 | } |
sawai | 0:3bd8aecadafc | 123 | return true; |
sawai | 0:3bd8aecadafc | 124 | } |
sawai | 0:3bd8aecadafc | 125 | return false; |
sawai | 0:3bd8aecadafc | 126 | } |
sawai | 0:3bd8aecadafc | 127 | |
sawai | 0:3bd8aecadafc | 128 | void Omuni::_set_speed() |
sawai | 0:3bd8aecadafc | 129 | { |
sawai | 0:3bd8aecadafc | 130 | //if(renew_speed()) |
sawai | 0:3bd8aecadafc | 131 | { |
sawai | 0:3bd8aecadafc | 132 | double _vx, _vy; |
sawai | 0:3bd8aecadafc | 133 | _vx = tvx * cos(theta) + tvy * sin(theta); |
sawai | 0:3bd8aecadafc | 134 | _vy = -1 * tvx * sin(theta) + tvy * cos(theta); |
sawai | 0:3bd8aecadafc | 135 | v[0] = (-1.0 * _vx ) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 136 | v[1] = ( 0.5 * _vx - 0.866 * _vy) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 137 | v[2] = ( 0.5 * _vx + 0.866 * _vy) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 138 | v[0] += (R * omega) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 139 | v[1] += (R * omega) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 140 | v[2] += (R * omega) / (3.1416 * d); |
sawai | 0:3bd8aecadafc | 141 | |
sawai | 0:3bd8aecadafc | 142 | |
sawai | 0:3bd8aecadafc | 143 | for(int i = 0; i < 3; i++) |
sawai | 0:3bd8aecadafc | 144 | { |
sawai | 0:3bd8aecadafc | 145 | o[i]->set_speed(v[i]); |
sawai | 0:3bd8aecadafc | 146 | } |
sawai | 0:3bd8aecadafc | 147 | } |
sawai | 0:3bd8aecadafc | 148 | } |
sawai | 0:3bd8aecadafc | 149 | |
sawai | 0:3bd8aecadafc | 150 | void Omuni::drive() |
sawai | 0:3bd8aecadafc | 151 | { |
sawai | 0:3bd8aecadafc | 152 | renew_theta(); |
sawai | 0:3bd8aecadafc | 153 | |
sawai | 0:3bd8aecadafc | 154 | _set_speed(); |
sawai | 0:3bd8aecadafc | 155 | |
sawai | 0:3bd8aecadafc | 156 | for(int i = 0; i < 3; i++) |
sawai | 0:3bd8aecadafc | 157 | { |
sawai | 0:3bd8aecadafc | 158 | char duty = o[i]->get_duty(); |
sawai | 0:3bd8aecadafc | 159 | i2c->write(addr[i], &duty, 1); |
sawai | 0:3bd8aecadafc | 160 | } |
sawai | 0:3bd8aecadafc | 161 | } |
sawai | 0:3bd8aecadafc | 162 | |
sawai | 0:3bd8aecadafc | 163 | void Omuni::reset_theta() |
sawai | 0:3bd8aecadafc | 164 | { |
sawai | 0:3bd8aecadafc | 165 | theta = 0.0; |
sawai | 0:3bd8aecadafc | 166 | } |
sawai | 0:3bd8aecadafc | 167 | |
sawai | 0:3bd8aecadafc | 168 | double Omuni::get_theta() |
sawai | 0:3bd8aecadafc | 169 | { |
sawai | 0:3bd8aecadafc | 170 | return theta; |
sawai | 0:3bd8aecadafc | 171 | } |
sawai | 0:3bd8aecadafc | 172 | |
sawai | 0:3bd8aecadafc | 173 | double Omuni::get_vx() |
sawai | 0:3bd8aecadafc | 174 | { |
sawai | 0:3bd8aecadafc | 175 | return vx; |
sawai | 0:3bd8aecadafc | 176 | } |
sawai | 0:3bd8aecadafc | 177 | |
sawai | 0:3bd8aecadafc | 178 | double Omuni::get_vy() |
sawai | 0:3bd8aecadafc | 179 | { |
sawai | 0:3bd8aecadafc | 180 | return vy; |
sawai | 0:3bd8aecadafc | 181 | } |