shinji sawai / Mbed 2 deprecated omuni_speed_pid

omuni

Dependencies:   mbed nucleo_rotary_encoder

omuni/omuni.cpp@0:6da7d0e457a2, 2017-08-17 (annotated)

Committer:
sawai
Date:
Thu Aug 17 03:49:34 2017 +0000
Revision:
0:6da7d0e457a2
omuni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawai 0:6da7d0e457a2 1 #include "mbed.h"
sawai 0:6da7d0e457a2 2 #include "omuni.hpp"
sawai 0:6da7d0e457a2 3 #include "speed_control.hpp"
sawai 0:6da7d0e457a2 4
sawai 0:6da7d0e457a2 5 omuni::omuni(I2C *i, TIM_TypeDef *timer_type0, TIM_TypeDef *timer_type1, TIM_TypeDef *timer_type2, size_t pulse_per_revol, float _cy_ms, int a[3], float _R, float _d)
sawai 0:6da7d0e457a2 6 {
sawai 0:6da7d0e457a2 7 i2c = i;
sawai 0:6da7d0e457a2 8 o[0] = new speed_control(timer_type0, pulse_per_revol, _cy_ms);
sawai 0:6da7d0e457a2 9 o[1] = new speed_control(timer_type1, pulse_per_revol, _cy_ms);
sawai 0:6da7d0e457a2 10 o[2] = new speed_control(timer_type2, pulse_per_revol, _cy_ms);
sawai 0:6da7d0e457a2 11 R = _R; d = _d;
sawai 0:6da7d0e457a2 12 vx = 0.0f; vy = 0.0f; omega = 0.0f;
sawai 0:6da7d0e457a2 13 for(int i = 0; i < 3; i++)
sawai 0:6da7d0e457a2 14 {
sawai 0:6da7d0e457a2 15 addr[i] = a[i];
sawai 0:6da7d0e457a2 16 v[i] = 0.0f;
sawai 0:6da7d0e457a2 17 }
sawai 0:6da7d0e457a2 18 time.start(); t.start();
sawai 0:6da7d0e457a2 19 f = false;
sawai 0:6da7d0e457a2 20 }
sawai 0:6da7d0e457a2 21 void omuni::set_pid(int n, float _kp, float _ki, float _kd)
sawai 0:6da7d0e457a2 22 {
sawai 0:6da7d0e457a2 23 if(n >= 0 && n <= 2)
sawai 0:6da7d0e457a2 24 {
sawai 0:6da7d0e457a2 25 o[n]->set_pid(_kp, _ki, _kd);
sawai 0:6da7d0e457a2 26 }
sawai 0:6da7d0e457a2 27 }
sawai 0:6da7d0e457a2 28
sawai 0:6da7d0e457a2 29
sawai 0:6da7d0e457a2 30 void omuni::set_speed(float _vx, float _vy)
sawai 0:6da7d0e457a2 31 {
sawai 0:6da7d0e457a2 32 tvx = _vx; tvy = _vy;
sawai 0:6da7d0e457a2 33 }
sawai 0:6da7d0e457a2 34
sawai 0:6da7d0e457a2 35 void omuni::set_speed(float _vx, float _vy, float _omega)
sawai 0:6da7d0e457a2 36 {
sawai 0:6da7d0e457a2 37 set_speed(_vx, _vy);
sawai 0:6da7d0e457a2 38 omega = _omega;
sawai 0:6da7d0e457a2 39 }
sawai 0:6da7d0e457a2 40
sawai 0:6da7d0e457a2 41 void omuni::set_speed(float _vx, float _vy, float _omega, bool _f)
sawai 0:6da7d0e457a2 42 {
sawai 0:6da7d0e457a2 43 set_speed(_vx, _vy, _omega);
sawai 0:6da7d0e457a2 44 f = _f;
sawai 0:6da7d0e457a2 45 }
sawai 0:6da7d0e457a2 46
sawai 0:6da7d0e457a2 47 void omuni::renew_theta()
sawai 0:6da7d0e457a2 48 {
sawai 0:6da7d0e457a2 49 static float now_time, pre_time;
sawai 0:6da7d0e457a2 50 now_time = time.read();
sawai 0:6da7d0e457a2 51
sawai 0:6da7d0e457a2 52 if(now_time - pre_time < 0.1f)
sawai 0:6da7d0e457a2 53 {
sawai 0:6da7d0e457a2 54 if(f == true)
sawai 0:6da7d0e457a2 55 {
sawai 0:6da7d0e457a2 56 theta += (omega * (now_time - pre_time)) * THETA_ADDJUST;
sawai 0:6da7d0e457a2 57 }
sawai 0:6da7d0e457a2 58 else
sawai 0:6da7d0e457a2 59 {
sawai 0:6da7d0e457a2 60 theta = 0.0f;
sawai 0:6da7d0e457a2 61 }
sawai 0:6da7d0e457a2 62 }
sawai 0:6da7d0e457a2 63 if(now_time > 1800)
sawai 0:6da7d0e457a2 64 {
sawai 0:6da7d0e457a2 65 time.reset();
sawai 0:6da7d0e457a2 66 pre_time = 0.0f;
sawai 0:6da7d0e457a2 67 }
sawai 0:6da7d0e457a2 68 pre_time = now_time;
sawai 0:6da7d0e457a2 69 }
sawai 0:6da7d0e457a2 70
sawai 0:6da7d0e457a2 71 bool omuni::renew_speed()
sawai 0:6da7d0e457a2 72 {
sawai 0:6da7d0e457a2 73 const double a = 10;
sawai 0:6da7d0e457a2 74 static float now_time, pre_time;
sawai 0:6da7d0e457a2 75 now_time = time.read();
sawai 0:6da7d0e457a2 76 if(now_time - pre_time > 0.01f)
sawai 0:6da7d0e457a2 77 {
sawai 0:6da7d0e457a2 78 pre_time = now_time;
sawai 0:6da7d0e457a2 79
sawai 0:6da7d0e457a2 80 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:6da7d0e457a2 81 {
sawai 0:6da7d0e457a2 82 vx = tvx; vy = tvy;
sawai 0:6da7d0e457a2 83 }
sawai 0:6da7d0e457a2 84 else
sawai 0:6da7d0e457a2 85 {
sawai 0:6da7d0e457a2 86 float x, y;
sawai 0:6da7d0e457a2 87 x = tvx - vx;
sawai 0:6da7d0e457a2 88 y = tvy - vy;
sawai 0:6da7d0e457a2 89 vx += a * 0.01 * x / sqrt(x * x + y * y);
sawai 0:6da7d0e457a2 90 vy += a * 0.01 * y / sqrt(x * x + y * y);
sawai 0:6da7d0e457a2 91 }
sawai 0:6da7d0e457a2 92 return true;
sawai 0:6da7d0e457a2 93 }
sawai 0:6da7d0e457a2 94 return false;
sawai 0:6da7d0e457a2 95 }
sawai 0:6da7d0e457a2 96
sawai 0:6da7d0e457a2 97 void omuni::_set_speed()
sawai 0:6da7d0e457a2 98 {
sawai 0:6da7d0e457a2 99 //if(renew_speed())
sawai 0:6da7d0e457a2 100 {
sawai 0:6da7d0e457a2 101 float _vx, _vy;
sawai 0:6da7d0e457a2 102 _vx = tvx * cos(theta) + tvy * sin(theta);
sawai 0:6da7d0e457a2 103 _vy = -1 * tvx * sin(theta) + tvy * cos(theta);
sawai 0:6da7d0e457a2 104 v[0] = (-1.0f * _vx ) / (3.1416f * d);
sawai 0:6da7d0e457a2 105 v[1] = ( 0.5f * _vx - 0.866f * _vy) / (3.1416f * d);
sawai 0:6da7d0e457a2 106 v[2] = ( 0.5f * _vx + 0.866f * _vy) / (3.1416f * d);
sawai 0:6da7d0e457a2 107 v[0] += (R * omega) / (3.1416f * d);
sawai 0:6da7d0e457a2 108 v[1] += (R * omega) / (3.1416f * d);
sawai 0:6da7d0e457a2 109 v[2] += (R * omega) / (3.1416f * d);
sawai 0:6da7d0e457a2 110
sawai 0:6da7d0e457a2 111
sawai 0:6da7d0e457a2 112 for(int i = 0; i < 3; i++)
sawai 0:6da7d0e457a2 113 {
sawai 0:6da7d0e457a2 114 o[i]->set_speed(v[i]);
sawai 0:6da7d0e457a2 115 }
sawai 0:6da7d0e457a2 116 }
sawai 0:6da7d0e457a2 117 }
sawai 0:6da7d0e457a2 118
sawai 0:6da7d0e457a2 119 void omuni::drive()
sawai 0:6da7d0e457a2 120 {
sawai 0:6da7d0e457a2 121 renew_theta();
sawai 0:6da7d0e457a2 122
sawai 0:6da7d0e457a2 123 _set_speed();
sawai 0:6da7d0e457a2 124
sawai 0:6da7d0e457a2 125 for(int i = 0; i < 3; i++)
sawai 0:6da7d0e457a2 126 {
sawai 0:6da7d0e457a2 127 char duty = o[i]->get_duty();
sawai 0:6da7d0e457a2 128 i2c->write(addr[i], &duty, 1);
sawai 0:6da7d0e457a2 129 }
sawai 0:6da7d0e457a2 130 }
sawai 0:6da7d0e457a2 131
sawai 0:6da7d0e457a2 132 void omuni::reset_theta()
sawai 0:6da7d0e457a2 133 {
sawai 0:6da7d0e457a2 134 theta = 0.0f;
sawai 0:6da7d0e457a2 135 }
sawai 0:6da7d0e457a2 136
sawai 0:6da7d0e457a2 137 float omuni::get_theta()
sawai 0:6da7d0e457a2 138 {
sawai 0:6da7d0e457a2 139 return theta;
sawai 0:6da7d0e457a2 140 }
sawai 0:6da7d0e457a2 141
sawai 0:6da7d0e457a2 142 float omuni::get_vx()
sawai 0:6da7d0e457a2 143 {
sawai 0:6da7d0e457a2 144 return vx;
sawai 0:6da7d0e457a2 145 }
sawai 0:6da7d0e457a2 146
sawai 0:6da7d0e457a2 147 float omuni::get_vy()
sawai 0:6da7d0e457a2 148 {
sawai 0:6da7d0e457a2 149 return vy;
sawai 0:6da7d0e457a2 150 }