shinji sawai / omuni

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?

UserRevisionLine numberNew 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 }