Akito / Mbed 2 deprecated ROBOCON2017_archan

あーちゃん制御

Dependencies:   FastPWM cal_PID mbed nucleo_rotary_encoder

omuni/omuni.cpp@1:d6e30aa7bc5b, 2017-09-25 (annotated)

Committer:
Akito914
Date:
Mon Sep 25 08:23:24 2017 +0000
Revision:
1:d6e30aa7bc5b
Parent:
0:4346c5764e83 
cal_PID???????

Who changed what in which revision?

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