![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
omuni
Dependencies: mbed nucleo_rotary_encoder
Revision 0:6da7d0e457a2, committed 2017-08-17
- Comitter:
- sawai
- Date:
- Thu Aug 17 03:49:34 2017 +0000
- Commit message:
- omuni
Changed in this revision
diff -r 000000000000 -r 6da7d0e457a2 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,96 @@ +#include "mbed.h" +#include "omuni.hpp" + +int addr[] = {0x10, 0x12, 0x14}; +int armAddr[] = {0x16, 0x18}; +bool arm = false; + +Serial p(USBTX, USBRX); +Serial pc(PA_11, PA_12); +I2C i2cMaster(D14, D15); +// archan +omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f); + +float vx, vy, ome; +bool f, pre_f; +char recv[3] = {0}; + +void pc_rx() +{ + char rtemp = pc.getc(); + + if((rtemp & 0b11000000) == 0b00000000) recv[0] = rtemp; + else if((rtemp & 0b11000000) == 0b01000000) recv[1] = rtemp; + else if((rtemp & 0b11000000) == 0b10000000) recv[2] = rtemp; + + float direc = (recv[0] & 0x0f) * 3.141592 / 8.0; + float speed = 0.8 * ((recv[0] & 0b00110000) >> 4); + vx = speed * cos(direc) * -1; + vy = speed * sin(direc) * -1; + + if(recv[2] & 0b1) + { + arm = true; + } + else + { + arm = false; + } + + if(recv[1] & 0b11000) + { + f = false; + ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0; + if(recv[1] & 0b100000) ome *= -1; + } + else + { + if(recv[1] & 0b11) + { + f = true; +// if(pre_f == false) omuni.reset_theta(); + } + else + { + f = false; + } + ome = (recv[1] & 0b11) * 3.141592 / 2.0; + if((recv[1] & 0b100)) ome *= -1; + } + pre_f = f; +} + +int main() +{ + p.baud(115200); + pc.baud(9600); + pc.printf("Hello!\n"); + pc.attach(pc_rx, Serial::RxIrq); + omuni.set_speed(0.0f, 0.0f); + // archan + omuni.set_pid(0, 3.0f, 0.07f, 0.05f); + omuni.set_pid(1, 3.0f, 0.07f, 0.05f); + omuni.set_pid(2, 3.0f, 0.07f, 0.05f); + + while(1) + { + wait(0.001); + omuni.set_speed(vx, vy, ome, f); + omuni.drive(); + + if(arm) + { + char send = -127; + i2cMaster.write(armAddr[0], &send, 1); + i2cMaster.write(armAddr[1], &send, 1); + } + else + { + char send = 0; + i2cMaster.write(armAddr[0], &send, 1); + i2cMaster.write(armAddr[1], &send, 1); + } + + p.printf("%f\n", omuni.get_theta()); + } +} \ No newline at end of file
diff -r 000000000000 -r 6da7d0e457a2 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/f9eeca106725 \ No newline at end of file
diff -r 000000000000 -r 6da7d0e457a2 omuni/omuni.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni/omuni.cpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,150 @@ +#include "mbed.h" +#include "omuni.hpp" +#include "speed_control.hpp" + +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) +{ + i2c = i; + 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); + R = _R; d = _d; + vx = 0.0f; vy = 0.0f; omega = 0.0f; + for(int i = 0; i < 3; i++) + { + addr[i] = a[i]; + v[i] = 0.0f; + } + time.start(); t.start(); + f = false; +} +void omuni::set_pid(int n, float _kp, float _ki, float _kd) +{ + if(n >= 0 && n <= 2) + { + o[n]->set_pid(_kp, _ki, _kd); + } +} + + +void omuni::set_speed(float _vx, float _vy) +{ + tvx = _vx; tvy = _vy; +} + +void omuni::set_speed(float _vx, float _vy, float _omega) +{ + set_speed(_vx, _vy); + omega = _omega; +} + +void omuni::set_speed(float _vx, float _vy, float _omega, bool _f) +{ + set_speed(_vx, _vy, _omega); + f = _f; +} + +void omuni::renew_theta() +{ + static float 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 float now_time, pre_time; + now_time = time.read(); + if(now_time - pre_time > 0.01f) + { + 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 + { + float 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()) + { + float _vx, _vy; + _vx = tvx * cos(theta) + tvy * sin(theta); + _vy = -1 * tvx * sin(theta) + tvy * cos(theta); + v[0] = (-1.0f * _vx ) / (3.1416f * d); + v[1] = ( 0.5f * _vx - 0.866f * _vy) / (3.1416f * d); + v[2] = ( 0.5f * _vx + 0.866f * _vy) / (3.1416f * d); + v[0] += (R * omega) / (3.1416f * d); + v[1] += (R * omega) / (3.1416f * d); + v[2] += (R * omega) / (3.1416f * 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.0f; +} + +float omuni::get_theta() +{ + return theta; +} + +float omuni::get_vx() +{ + return vx; +} + +float omuni::get_vy() +{ + return vy; +} \ No newline at end of file
diff -r 000000000000 -r 6da7d0e457a2 omuni/omuni.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni/omuni.hpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,36 @@ +#ifndef _omuni_ +#define _omuni_ + +#include "speed_control.hpp" + +#define THETA_ADDJUST 0.69f + +class omuni +{ +private: + I2C *i2c; + speed_control *o[3]; + Timer time, t; + float tvx, tvy, tomega; + float vx, vy, omega, theta; + float R, d; + int addr[3]; + float v[3]; + bool f; +public: + 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); + void set_pid(int n, float _kp, float _ki, float _kd); + void set_speed(float vx, float vy); + void set_speed(float vx, float vy, float omega); + void set_speed(float _vx, float _vy, float _omega, bool _f); + void renew_theta(); + bool renew_speed(); + void _set_speed(); + void drive(); + void reset_theta(); + float get_theta(); + float get_vx(); + float get_vy(); +}; + +#endif
diff -r 000000000000 -r 6da7d0e457a2 omuni/speed_control/nucleo_rotary_encoder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni/speed_control/nucleo_rotary_encoder.lib Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/inst/code/nucleo_rotary_encoder/#684e1604e5ea
diff -r 000000000000 -r 6da7d0e457a2 omuni/speed_control/speed_control.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni/speed_control/speed_control.cpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,97 @@ +#include "mbed.h" +#include "speed_control.hpp" +#include "rotary_encoder_ab_phase.hpp" + +speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms) +{ + e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol); + pulse_per_revol = _pulse_per_revol; + cy_ms = _cy_ms; + f1 = f2 = 0; + target_value = 0.0f; + + e->start(); + t.start(); +} + +void speed_control::set_speed(float _target_value) +{ + target_value = _target_value; +} + +void speed_control::set_pid(float _kp, float _ki, float _kd) +{ + kp = _kp; ki = _ki; kd = _kd; + f1 = kp + ki / 2.0f + kd; + f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd; + + e->reset(); + t.reset(); + time_counter = 0; + pre_counts = 0; +} + +bool speed_control::check_get_speed() +{ + if(t.read_ms() > time_counter) + { + time_counter += cy_ms; + + for(int i = 9; i > 0; i--) + { + e_value[i] = e_value[i-1]; + } + int32_t ec = e->get_counts(); + e_value[0] = ec - pre_counts; + if(ec > 20000 || ec < -20000) + { + e->reset(); + pre_counts = pre_counts - ec; + } + else + { + pre_counts = ec; + } + int32_t esum = 0; + for(int i = 0; i < 10; i++) + { + esum += e_value[i]; + } + sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000; + + return true; + } + else + { + return false; + } +} + +char speed_control::get_duty() +{ + if(t.read() > 1800) + { + t.reset(); + time_counter = 0; + } + if(check_get_speed()) + { + diff[0] = sensor_value - target_value; + dm = f1 * diff[0] + j; + m = m + dm; + + int m_int = (int)m * -1; + if(m_int <= -126) m_int = -126; + if(m_int >= 126) m_int = 126; + duty = m_int; + + j = f2 * diff[0] + kd * diff[1]; + diff[1] = diff[0]; + } + return duty; +} + +float speed_control::get_speed() +{ + return sensor_value; +} \ No newline at end of file
diff -r 000000000000 -r 6da7d0e457a2 omuni/speed_control/speed_control.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni/speed_control/speed_control.hpp Thu Aug 17 03:49:34 2017 +0000 @@ -0,0 +1,29 @@ +#ifndef __SPEED_CONTROL__ +#define __SPEED_CONTROL__ + +#include "mbed.h" +#include "rotary_encoder_ab_phase.hpp" + +class speed_control +{ +private: + rotary_encoder_ab_phase *e; + Timer t; + float kp, ki, kd; + float f1, f2, diff[2], m, dm, j; + float pulse_per_revol, cy_ms; + float target_value, sensor_value; + unsigned long time_counter; + int32_t e_value[10], pre_counts; + char duty; + +public: + speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms); + void set_speed(float _target_value); + void set_pid(float _kp, float _ki, float _kd); + bool check_get_speed(); + char get_duty(); + float get_speed(); +}; + +#endif \ No newline at end of file