Diff: omuni.cpp

Revision:

0:3bd8aecadafc

--- /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;
+}