Revision 0:ee7e9405e1c7, committed 2021-04-14

Comitter:

e2011220

Date:

Wed Apr 14 07:26:19 2021 +0000

Commit message:

first

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eigen.lib	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/jsoh91/code/Eigen/#3b8049da21b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Reset.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,10 @@
+#include <mbed.h>
+#include <Reset.h>
+
+void Reset_Mbed::Reset(double time,double limit)
+{
+    if (time>=limit){
+        printf("RESET  \n\r");
+        NVIC_SystemReset();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Reset.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,12 @@
+#ifndef RESET
+#define RESET
+#include <mbed.h>
+
+class Reset_Mbed
+{
+public:
+    virtual void Reset(double time,double lim);
+};
+
+#endif
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "math.h"
+#include "mechanam.h"
+#include "Reset.h"
+
+Serial Twelite(D5,D4,115200);
+
+Mechanam gx_d_l(D7,D12);//1 #1/3N
+Mechanam fx_d_l(A3,D10);//2 #3/2
+Mechanam gx_d_r(D9,D3);//3 #1/1
+Mechanam fx_d_r(D2,D8);//4 #16/1
+
+Mbed_MD elev(A2,D11);
+
+DigitalOut fire(D6);
+
+char buffer[12];
+double x;
+double y;
+double x_t;
+double y_t;
+int8_t SEL;
+int8_t SEL_T;
+bool error_fa=false;
+bool check=false;
+bool Yadama=false;
+bool Yadama_check=false;
+Timer error_time;
+
+Reset_Mbed Reset_t;
+
+void receive()
+{
+    for (int i=0; i<=11; i++) {
+        buffer[i]=Twelite.getc();
+        if (error_fa==true and buffer[0]!='[')return;
+    }
+    if (buffer[0]=='[' and buffer[11]=='\0' and int16_t(buffer[9])<=1 and int16_t(buffer[10])<=1) {
+        int16_t x_get=(buffer[1]<<8)+buffer[2];
+        int16_t y_get=(buffer[3]<<8)+buffer[4];
+        int16_t turnx_get=(buffer[5]<<8)+buffer[6];
+        int16_t turny_get=(buffer[7]<<8)+buffer[8];
+        SEL=buffer[9];
+        SEL_T=buffer[10];
+        x=(double)(x_get)/1000.0;
+        y=(double)(y_get)/1000.0;
+        x_t=(double)(turnx_get)/1000.0;
+        y_t=(double)(turny_get)/1000.0;
+        check=true;
+        error_fa=false;
+        error_time.reset();
+    } else {
+        check=false;
+        error_fa=true;
+    }
+}
+
+int main()
+{
+    Twelite.attach(receive,Serial::RxIrq);
+
+    while(1) {
+        // ----------------ここから矢玉の処理----------------
+        if ( (SEL_T==1) and Yadama==false) Yadama=true;
+        if ( (SEL_T==0) and Yadama==true) Yadama_check=true;
+        while (Yadama_check==true) {
+            fx_d_r.free();
+            fx_d_l.free();
+            gx_d_r.free();
+            gx_d_l.free();
+
+            fire=(SEL==1 ? 1:0);
+
+            if (-0.10<y_t and y_t<0.10)y_t=0;
+            elev.drive(-y_t);
+
+            printf("mode:Y SEL=%d,Y=%3.2f\n\r",SEL,-y_t);
+            
+            if (error_fa==true and error_time.read()==0)error_time.start();
+            if (error_time.read()>=0.5f)Twelite.putc('e');
+            Reset_t.Reset(error_time.read(),1.0);
+            
+            if ( (SEL_T==1) and Yadama==true)Yadama=false;
+            if ( (SEL_T==0) and Yadama==false)Yadama_check=false;
+        }
+        // ----------------ここまで矢玉の処理----------------
+
+        if(error_fa==true and error_time.read()==0)error_time.start();
+        
+        // --------------ここからメカナムの処理---------------
+
+        if (check==true) {
+            elev.free();
+            fx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
+            fx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
+            gx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
+            gx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
+            printf("mode:M x=%3.2f y=%3.2f x_t=%3.2f\n\r",x,y,x_t);
+            check=false;
+            error_fa=true;
+        } else {
+            error_fa=true;
+        }
+
+        if (error_time.read()>=0.5f)Twelite.putc('e');
+        Reset_t.Reset(error_time.read(),1.0);
+        // --------------ここまでメカナムの処理---------------
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mechanam.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,45 @@
+#include <mbed.h>
+#include <mechanam.h>
+#include <math.h>
+
+Mechanam::Mechanam(PinName pwmPin,PinName dirPin):_moter(pwmPin,dirPin){}
+
+void Mechanam::drive(double x,double y,double x_t,double magni,double magni_t,double ex,double ey,double ex_t,char location)
+{
+    double speed=sqrt(x*x+y*y)*magni;
+    double rad=atan2(y,-x);
+    double fx=speed*sqrt(2.0)*((sin(rad)-cos(rad))/4.0);
+    double gx=speed*sqrt(2.0)*((sin(rad)+cos(rad))/4.0);
+    
+    if (-ex_t>x_t or x_t>ex_t) {
+        _moter.drive(x_t*magni_t);
+    }else if (-ex<=x and x<=ex and -ey<=y and y<=ey) {
+        _moter.free();
+    }else {
+        if (location=='f')_moter.drive(fx);
+        else if (location=='g')_moter.drive(gx);
+    }
+}
+
+void Mechanam::free()
+{
+    _moter.free();
+}
+
+
+/*
+fx=(Speed*√2)*((sin(x)-cos(x))/4.0
+gx=(Speed*√2)*((sin(x)-cos(x))/4.0
+
+This is a ASCII art of Mechanam
+     -              -
+    | |←fx         | |←gx
+    | |            | |
+     -              -
+ 
+ 
+     -              -
+    | |←gx         | |←fx
+    | |            | |
+     -              -
+ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mechanam.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,17 @@
+#ifndef MECHANAMU
+#define MECHANAMU
+#include <mbed.h>
+#include <shared/Mbed_MD/Mbed_MD.h>
+
+class Mechanam
+{
+public:
+    Mechanam(PinName pwmPin,PinName dirPin);
+    virtual void drive(double x,double y,double x_turn,double magni,double magni_turn,double error_x,double error_y,double error_x_turn,char location);
+    virtual void free();
+
+private:
+    Mbed_MD _moter;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/AutomaticMovement/AutomaticMovement.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,16 @@
+#include "AutomaticMovement.h"
+
+AutomaticMovement::AutomaticMovement(Movement *movement)
+:_movement(movement)
+{}
+
+Eigen::Vector3d AutomaticMovement::move_automatic(Eigen::Vector3d target_position, Eigen::Vector3d current_position)
+{
+	set_target_position(target_position);
+	return move_automatic(current_position);
+}
+
+void AutomaticMovement::set_target_position(Eigen::Vector3d target_position)
+{
+	_target_position = target_position;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/AutomaticMovement/AutomaticMovement.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,24 @@
+#ifndef AUTOMATIC_MOVEMENT_H_
+#define AUTOMATIC_MOVEMENT_H_
+
+#include <shared/Movement/Movement.h>
+#include <shared/EigenCalculation/EigenCalculation.h>
+
+class AutomaticMovement
+{
+public:
+	AutomaticMovement(Movement *movement);
+	virtual ~AutomaticMovement(){}
+
+	virtual Eigen::Vector3d move_automatic(Eigen::Vector3d current_position) = 0;
+	Eigen::Vector3d move_automatic(Eigen::Vector3d target_position, Eigen::Vector3d current_position);
+
+	virtual void set_target_position(Eigen::Vector3d target_position);
+
+protected:
+	Movement *_movement;
+
+	Eigen::Vector3d _target_position;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/DamyMovement/DamyMovement.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,24 @@
+#ifndef DAMY_MOVEMENT_H_
+#define DAMY_MOVEMENT_H_
+
+#include <shared/Movement/Movement.h>
+
+class DamyMovement : public Movement
+{
+public:
+	virtual ~DamyMovement(){}
+	virtual void move(double x, double y, double yaw)
+	{
+		Eigen::Vector3d speed_move;
+		speed_move << x, y, yaw;
+		_speed_move = speed_move;
+	}
+	Eigen::Vector3d get_speed_move()
+	{
+		return _speed_move;
+	}
+private:
+	Eigen::Vector3d _speed_move;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Diff2/Diff2.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,46 @@
+#include "Diff2.h"
+
+Diff2::Diff2(MD *md[NUM])
+{
+    wheel[L] = new Wheel( md[L],  M_PI / 2.0,  M_PI);
+    wheel[R] = new Wheel( md[R], -M_PI / 2.0,  0.0);
+}
+
+Diff2::Diff2(MD *md_L, MD *md_R)
+{
+    wheel[L] = new Wheel( md_L,  M_PI / 2.0,  M_PI);
+    wheel[R] = new Wheel( md_R, -M_PI / 2.0,  0.0);
+}
+
+Diff2::Diff2(Wheel *wheel[NUM])
+{
+    for(int i = 0; i < NUM; i++){
+        this->wheel[i] = wheel[i];
+    }
+}
+
+void Diff2::move(double x, double y, double yaw)
+{
+    double move_angle, move_radius;
+    double duty[NUM];
+    double max_duty, limit_duty = 1.0;
+    
+    move_angle = atan2(y, x);
+    move_radius = sqrt( y * y + x * x );
+    
+    for(int i = 0; i < NUM; i++)
+        duty[i] = wheel[i]->wheel_speed(move_angle, move_radius, yaw);
+
+    // 平行移動と回転のうち、大きい方をmax_dutyに格納
+    if(move_radius > fabs(yaw))
+    	max_duty = move_radius;
+    else
+    	max_duty = fabs(yaw);
+
+	up_limit_balance(duty, NUM, max_duty);
+	down_limit_balance(duty, NUM, limit_duty);
+    
+    for(int i = 0; i < NUM; i++){
+        wheel[i]->drive(duty[i]);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Diff2/Diff2.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,27 @@
+#ifndef DIFF2_H_
+#define DIFF2_H_
+
+#include <shared/Movement/Movement.h>
+#include <shared/Wheel/Wheel.h>
+
+class Diff2 : public Movement
+{
+public:
+    enum
+    {
+        L,
+        R,
+        NUM
+    };
+
+    Diff2(MD *md[NUM]);
+    Diff2(MD *md_L, MD *md_R);
+    Diff2(Wheel *wheel[NUM]);
+
+    virtual void move(double x, double y, double yaw);
+
+private:
+    Wheel *wheel[NUM];
+}; 
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/EigenCalculation/EigenCalculation.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,45 @@
+#include "EigenCalculation.h"
+
+void EigenCalculation::invert_matrix(Eigen::Matrix3d *matrix3d)
+{
+	Eigen::FullPivLU< Eigen::Matrix3d > lu(*matrix3d);
+	(*matrix3d) = lu.inverse();
+}
+
+void EigenCalculation::yawling_correct(Eigen::Vector3d *vector3d)
+{
+	Eigen::Vector3d vector_arc;
+	double arc, vector_angle, radius;
+	double x, y, yaw;
+
+	x	= (*vector3d)(EigenCalculation::X);
+	y	= (*vector3d)(EigenCalculation::Y);
+	yaw = (*vector3d)(EigenCalculation::Yaw);
+
+	if(yaw == 0.0)
+		return;
+
+	arc = sqrt(x * x + y * y);
+	vector_angle = atan2(y, x);
+
+	radius = arc / yaw;
+
+	vector_arc(EigenCalculation::X) = radius * (1.0 - cos(yaw));
+	vector_arc(EigenCalculation::Y) = radius * sin(yaw);
+	vector_arc(EigenCalculation::Yaw) = yaw;
+
+	rotate_coordinate( &vector_arc, -(0.5 * M_PI - vector_angle) );
+
+	(*vector3d) = vector_arc;
+}
+
+void EigenCalculation::rotate_coordinate(Eigen::Vector3d *vector3d, double yaw)
+{
+	Eigen::Vector3d rotated_vector;
+	Eigen::Vector3d axis; axis << 0,0,1;
+	Eigen::Matrix3d rotation3d; rotation3d = Eigen::AngleAxisd(yaw, axis);
+
+	rotated_vector = rotation3d * (*vector3d);
+
+	(*vector3d) = rotated_vector;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/EigenCalculation/EigenCalculation.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,23 @@
+#ifndef EIGEN_CALCULATION_H_
+#define EIGEN_CALCULATION_H_
+
+#include <Dense.h>
+#include <LU.h>
+#include <math.h>
+
+namespace EigenCalculation
+{
+	enum{
+		X,
+		Y,
+		Yaw
+	};
+
+	void invert_matrix(Eigen::Matrix3d *matrix3d);
+	void yawling_correct(Eigen::Vector3d *vector3d);
+	void rotate_coordinate(Eigen::Vector3d *vector3d, double yaw);
+}
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/GCodeMovement/GCodeMovement.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,43 @@
+#include "GCodeMovement.h"
+
+GCodeMovement::GCodeMovement(AutomaticMovement *automatic_movement, Eigen::Vector3d *current_position, Eigen::Vector3d *target_position)
+	: _automatic_movement(automatic_movement), GCodeReader(current_position, target_position)
+{}
+
+Eigen::Vector3d GCodeMovement::move_g_code(Eigen::Vector3d current_position)
+{
+	Eigen::Vector3d speed_move;
+	Eigen::Vector3d difference;
+
+	*GCodeReader::_current_position = current_position;
+
+	if(_g_code_list.size() == 0)
+		_g_code_list.push(std::string("g0"));
+
+	read_g_code(_g_code_list.front());
+
+	difference = *GCodeReader::_target_position - *GCodeReader::_current_position;
+
+	if( (difference.array().abs() < _near_position.array()).all() )
+		_g_code_list.pop();
+
+	speed_move = _automatic_movement->move_automatic(*GCodeReader::_target_position, *GCodeReader::_current_position);
+
+	return speed_move;
+}
+
+void GCodeMovement::push_g_code(const std::string g_code)
+{
+	_g_code_list.push(g_code);
+}
+
+void GCodeMovement::reset_g_code()
+{
+	while(!_g_code_list.empty())
+		_g_code_list.pop();
+}
+
+void GCodeMovement::set_near_position(const Eigen::Vector3d &near_position)
+{
+	_near_position = near_position;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/GCodeMovement/GCodeMovement.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,26 @@
+#ifndef G_CODE_MOVEMENT_H_
+#define G_CODE_MOVEMENT_H_
+
+#include <shared/AutomaticMovement/AutomaticMovement.h>
+#include <shared/GCodeReader/GCodeReader.h>
+#include <queue>
+
+class GCodeMovement : public GCodeReader
+{
+public:
+	GCodeMovement(AutomaticMovement *automatic_movement, Eigen::Vector3d *current_position, Eigen::Vector3d *target_position);
+	virtual ~GCodeMovement(){}
+
+	Eigen::Vector3d move_g_code(Eigen::Vector3d current_position);
+
+	void push_g_code(const std::string g_code);
+	void reset_g_code();
+	void set_near_position(const Eigen::Vector3d &near_position);
+
+private:
+	AutomaticMovement *_automatic_movement;
+	std::queue<std::string> _g_code_list;
+	Eigen::Vector3d _near_position;
+
+};
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/GCodeReader/GCodeReader.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,103 @@
+#include "GCodeReader.h"
+GCodeReader::GCodeReader(Eigen::Vector3d *current_position, Eigen::Vector3d *target_position)
+	: _current_position(current_position), _target_position(target_position)
+{}
+
+void GCodeReader::read_g_code(const std::string &g_code)
+{
+	Eigen::Vector3d target_position;
+	std::vector<std::string> strings;
+
+	strings = split_space(g_code);
+
+	switch(strings[0][0]){
+	case 'g':
+	case 'G':
+		int code_num;
+		std::stringstream ss;
+
+		ss.str("");
+		ss.clear(std::stringstream::goodbit);
+		ss << strings[0];
+		ss.ignore();
+		ss >> code_num;
+
+		switch(code_num){
+		case 0:
+			_function_g_00(strings);
+			break;
+		case 4:
+			_function_g_04(strings);
+			break;
+		}
+		break;
+	}
+}
+
+void GCodeReader::_function_g_00(const std::vector<std::string> &strings)
+{
+	double value;
+	std::stringstream ss;
+	for(int i = 1; i < strings.size(); i++){
+
+		ss.str("");
+		ss.clear(std::stringstream::goodbit);
+		ss << strings[i];
+		ss.ignore();
+		ss >> value;
+
+		switch(strings[i][0]){
+		case 'x':
+		case 'X':
+			(*_target_position)(Movement::X) = value;
+			break;
+		case 'y':
+		case 'Y':
+			(*_target_position)(Movement::Y) = value;
+			break;
+		case 'z':
+		case 'Z':
+			(*_target_position)(Movement::Yaw) = value * M_PI;
+			break;
+		}
+	}
+}
+
+void GCodeReader::_function_g_04(const std::vector<std::string> &strings)
+{
+	double value;
+	std::stringstream ss;
+	ss.str("");
+	ss.clear(std::stringstream::goodbit);
+	ss << strings[1];
+	ss.ignore();
+	ss >> value;
+
+	switch(strings[1][0]){
+	case 'x':
+	case 'X':
+		wait(value);
+		break;
+	case 'p':
+	case 'P':
+		wait(value * 0.001);
+		break;
+	}
+}
+
+std::vector<std::string> GCodeReader::split_space(const std::string &str)
+{
+    return split(str, ' ');
+}
+
+
+std::vector<std::string> GCodeReader::split(const std::string &str, char sep)
+{
+    std::vector<std::string> strings;
+    std::stringstream ss(str);
+    std::string buffer;
+    while( std::getline(ss, buffer, sep) ) {
+        strings.push_back(buffer);
+    }
+    return strings;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/GCodeReader/GCodeReader.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,33 @@
+#ifndef G_CODE_READER_H_
+#define G_CODE_READER_H_
+
+#include <shared/Movement/Movement.h>
+
+#include <Dense.h>
+
+#include <vector>
+#include <string>
+#include <iostream>
+
+class GCodeReader
+{
+public:
+	GCodeReader(Eigen::Vector3d *current_position, Eigen::Vector3d *target_position);
+	virtual ~GCodeReader(){}
+
+	void read_g_code(const std::string &g_code);
+
+	std::vector<std::string> split_space(const std::string &str);
+	std::vector<std::string> split(const std::string &str, char sep);
+protected:
+
+	Eigen::Vector3d *_current_position;
+	Eigen::Vector3d *_target_position;
+
+private:
+
+	void _function_g_00(const std::vector<std::string> &strings);
+	void _function_g_04(const std::vector<std::string> &strings);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/I2CMD/I2CMD.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,83 @@
+
+#include <math.h>
+#include <shared/I2CMD/I2CMD.h>
+
+I2CMD::I2CMD(I2C *i2c_bus, int i2c_address, int motor_id, int retries)
+    : _i2c_bus(i2c_bus),_i2c_address(i2c_address),_motor_id(motor_id)
+	, _retries(retries), _error(true)
+{
+}
+
+void I2CMD::drive(double strength)
+{
+    uint8_t dir;
+    char data[2];
+
+    if(signbit(strength)) {
+        dir = 1;
+    } else {
+        dir = 0;
+    }
+
+    data[0] = dir;
+    data[1] = conv_strength_to_duty_u8(strength);
+
+    i2c_write('D', data, 2);
+}
+
+void I2CMD::brake(double strength)
+{
+    char data[1];
+    data[0] = conv_strength_to_duty_u8(strength);
+    i2c_write('B', data, 1);
+}
+
+void I2CMD::free()
+{
+    char data[1] = {0};
+    i2c_write('F', data, 1);
+}
+
+void I2CMD::set_retries(int retries)
+{
+	_retries = retries;
+}
+
+bool I2CMD::get_error()
+{
+	return _error;
+}
+
+bool I2CMD::i2c_write(int command, char *data, int length)
+{
+    const int send_length = 2 + length;
+    char send_data[send_length];
+    bool error = true;
+
+    send_data[0] = _motor_id;
+    send_data[1] = command;
+    for(int i = 0; i < length; i++) {
+        send_data[i + 2] = data[i];
+    }
+
+    for(int i = 0; i < _retries && error; i++) {
+    	error = _i2c_bus->write(_i2c_address, send_data, send_length);
+    }
+
+    _error = error;
+    return error;
+}
+
+uint8_t I2CMD::conv_strength_to_duty_u8(double strength)
+{
+    double duty;
+    uint8_t duty_u8;
+
+    duty = fabs(strength);
+    if(duty > 0.3) {
+        duty = 0.3;
+    }
+    duty_u8 = floor(duty * 255);
+
+    return duty_u8;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/I2CMD/I2CMD.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,30 @@
+#ifndef I2CMD_H_
+#define I2CMD_H_
+
+#include <mbed.h>
+#include <shared/MD/MD.h>
+
+class I2CMD : public MD
+{
+public:
+    I2CMD(I2C *i2c_bus, int i2c_address, int motor_id, int retries = 10);
+    virtual void drive(double);
+    virtual void brake(double);
+    virtual void free();
+
+    void set_retries(int);
+    bool get_error();
+
+private:
+    bool i2c_write(int command, char *data, int length);
+    uint8_t conv_strength_to_duty_u8(double);
+
+    I2C *_i2c_bus;
+    int _i2c_address;
+    int _motor_id;
+
+    int _retries;
+    bool _error;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/I2CMDMain/I2CMDMain.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,96 @@
+#include "I2CMDMain.h"
+
+#include <math.h>
+
+I2CMDMain::I2CMDMain(I2C *i2c_bus, int i2c_address, int motor_id, int retries)
+    : _i2c_bus(i2c_bus),_i2c_address(i2c_address),_motor_id(motor_id)
+	, _retries(retries), _error(true)
+{
+	for(int i = 0; i < 4; i++)
+		send_data[i] = 0;
+	send_flg = 0;
+	send_length = 0;
+}
+
+void I2CMDMain::drive(double strength)
+{
+    uint8_t dir;
+    char data[2];
+
+    if(signbit(strength)) {
+        dir = 1;
+    } else {
+        dir = 0;
+    }
+
+    data[0] = dir;
+    data[1] = conv_strength_to_duty_u8(strength);
+
+    i2c_write('D', data, 2);
+}
+
+void I2CMDMain::brake(double strength)
+{
+    char data[1];
+    data[0] = conv_strength_to_duty_u8(strength);
+    i2c_write('B', data, 1);
+}
+
+void I2CMDMain::free()
+{
+    char data[1] = {0};
+    i2c_write('F', data, 1);
+}
+
+void I2CMDMain::set_retries(int retries)
+{
+	_retries = retries;
+}
+
+bool I2CMDMain::get_error()
+{
+	return _error;
+}
+
+bool I2CMDMain::i2c_send()
+{
+	bool error = true;
+	if(send_flg){
+	    for(int i = 0; i < _retries && error; i++) {
+	    	error = _i2c_bus->write(_i2c_address, send_data, send_length);
+	    }
+		_error = error;
+		send_flg = 0;
+	}
+	wait(0.0001);
+	return error;
+}
+
+bool I2CMDMain::i2c_write(int command, char *data, int length)
+{
+    send_length = 2 + length;
+    int error = 0;
+
+    send_data[0] = _motor_id;
+    send_data[1] = command;
+    for(int i = 0; i < length; i++) {
+        send_data[i + 2] = data[i];
+    }
+    send_flg = 1;
+
+    return (bool)error;
+}
+
+uint8_t I2CMDMain::conv_strength_to_duty_u8(double strength)
+{
+    double duty;
+    uint8_t duty_u8;
+
+    duty = fabs(strength);
+    if(duty > 1.0) {
+        duty = 1.0;
+    }
+    duty_u8 = floor(duty * 255);
+
+    return duty_u8;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/I2CMDMain/I2CMDMain.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,36 @@
+#ifndef I2CMDMAIN_H_
+#define I2CMDMAIN_H_
+
+#include <mbed.h>
+#include <shared/MD/MD.h>
+
+class I2CMDMain : public MD
+{
+public:
+    I2CMDMain(I2C *i2c_bus, int i2c_address, int motor_id, int retries = 10);
+    virtual void drive(double);
+    virtual void brake(double);
+    virtual void free();
+
+    void set_retries(int);
+    bool get_error();
+
+    bool i2c_send();
+
+private:
+    bool i2c_write(int command, char *data, int length);
+    uint8_t conv_strength_to_duty_u8(double);
+
+    I2C *_i2c_bus;
+    int _i2c_address;
+    int _motor_id;
+
+    int _retries;
+    bool _error;
+
+    char send_data[4];
+    int send_length;
+    bool send_flg;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD/MD.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,14 @@
+#ifndef MD_H_
+#define MD_H_
+
+class MD
+{
+public:
+
+    virtual ~MD(){}
+    virtual void drive(double) = 0;
+    virtual void brake(double) = 0;
+    virtual void free() = 0;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,57 @@
+#include "MD_PID.h"
+
+MD_PID::MD_PID( MD *md, QEI *qei,
+                double k, double ti, double td,
+				double max_speed)
+                : _md(md), _qei(qei), _pid(new PI_D(k, ti, td))
+{
+    _duty = 0.0;
+    set_max_speed(max_speed);
+}
+
+void MD_PID::drive(double target_vel, double interval)
+{
+    double present_vel = read_vel();
+
+    if(fabs(target_vel) > _max_speed){
+        if(target_vel >= 0)
+            target_vel = _max_speed;
+        else
+            target_vel = -_max_speed;
+    }
+    //PID_Control::PID(present_vel, target_vel, interval);
+    //PID_Control::VPID(present_vel, target_vel, interval);
+    //PID::PI_D(present_vel, target_vel, interval);
+    _pid->set_state(present_vel, target_vel);
+    _duty = -1 * _pid->get_control();
+    
+    if( fabs(_duty) > 1 )
+        _duty /= fabs(_duty);
+        
+    _md->drive(_duty);
+    //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
+}
+
+double MD_PID::get_duty(){
+	return _duty;
+}
+
+void MD_PID::brake(double target)
+{
+    
+}
+
+void MD_PID::free()
+{
+    
+}
+
+void MD_PID::set_max_speed(double max_speed)
+{
+	_max_speed = max_speed;
+}
+
+double MD_PID::read_vel()
+{
+    return _qei->get_ang_vel();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.cpp.txt	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,58 @@
+#include "MD_PID.h"
+
+MD_PID::MD_PID( MD *md, QEI *qei,
+                double kp, double ki, double kd,
+				double max_speed)
+                : _md(md), _qei(qei), PID_Control(kp, ki, kd)
+{
+    _duty = 0.0;
+    set_max_speed(max_speed);
+}
+
+void MD_PID::drive(double target_vel, double interval)
+{
+    double present_vel = read_vel();
+
+    if(fabs(target_vel) > _max_speed){
+        if(target_vel >= 0)
+            target_vel = _max_speed;
+        else
+            target_vel = -_max_speed;
+    }
+    //PID_Control::PID(present_vel, target_vel, interval);
+    //PID_Control::VPID(present_vel, target_vel, interval);
+    PID_Control::PI_D(present_vel, target_vel, interval);
+    _duty = PID_Control::get_control();
+    
+    //printf("%lf\r\n", _duty);
+    
+    if( fabs(_duty) > 1 )
+        _duty /= fabs(_duty);
+        
+    _md->drive(_duty);
+    //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
+}
+
+double MD_PID::get_duty(){
+	return _duty;
+}
+
+void MD_PID::brake(double target)
+{
+    
+}
+
+void MD_PID::free()
+{
+    
+}
+
+void MD_PID::set_max_speed(double max_speed)
+{
+	_max_speed = max_speed;
+}
+
+double MD_PID::read_vel()
+{
+    return _qei->get_ang_vel();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,33 @@
+#ifndef MD_PID_H_
+#define MD_PID_H_
+
+#include <shared/MD/MD.h>
+#include <shared/QEI/QEI.h>
+#include "PID.h"
+#include "PPID.h"
+#include "PI_D.h"
+
+class MD_PID
+{
+public:
+    MD_PID( MD *md, QEI *qei,
+            double k = 0, double ti = 0, double td = 0,
+			double max_speed = 0);
+    virtual void drive(double target, double interval);
+    virtual void brake(double target);
+    virtual void free();
+    void set_max_speed(double max_speed);
+    
+    double get_duty();
+    
+private:
+    MD *_md;
+    QEI *_qei;
+    PID *_pid;
+    double _duty;
+    double _max_speed;
+    
+    double read_vel();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.h.txt	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,30 @@
+#ifndef MD_PID_H_
+#define MD_PID_H_
+
+#include <shared/MD/MD.h>
+#include <shared/QEI/QEI.h>
+#include <shared/PID_Control/PID_Control.h>
+
+class MD_PID : public PID_Control
+{
+public:
+    MD_PID( MD *md, QEI *qei,
+            double kp = 0, double ki = 0, double kd = 0,
+			double max_speed = 0);
+    virtual void drive(double target, double interval);
+    virtual void brake(double target);
+    virtual void free();
+    void set_max_speed(double max_speed);
+    
+    double get_duty();
+    
+private:
+    MD *_md;
+    QEI *_qei;
+    double _duty;
+    double _max_speed;
+    
+    double read_vel();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Mbed_MD/Mbed_MD.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,29 @@
+#include <mbed.h>
+#include <shared/Mbed_MD/Mbed_MD.h>
+
+Mbed_MD::Mbed_MD(PinName pwmPin,PinName dirPin): _pwmOut(pwmPin), _dirOut(dirPin)
+{
+    _pwmOut.period(0.00001);
+}
+
+void Mbed_MD::drive(double duty)
+{
+    if(duty >= 0){   
+        _pwmOut.write(duty);
+        _dirOut = 0;
+    }else
+    if(duty < 0){    
+        _pwmOut.write(-duty);
+        _dirOut = 1;
+    }
+}
+
+void Mbed_MD::brake(double duty)
+{
+}
+
+void Mbed_MD::free()
+{
+    _pwmOut.write(0);
+    _dirOut = 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Mbed_MD/Mbed_MD.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,23 @@
+#ifndef MBED_MD_H_
+#define MBED_MD_H_
+
+#include <mbed.h>
+#include <shared/MD/MD.h>
+
+class Mbed_MD : public MD
+{
+public:
+    Mbed_MD(PinName pwmPin,PinName dirPin);
+    virtual void drive(double);
+    virtual void brake(double);
+    virtual void free();
+
+private:
+    PwmOut _pwmOut;
+    DigitalOut _dirOut;
+};
+
+#endif
+    
+    
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Mecanum_4/Mecanum_4.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,50 @@
+#include "Mecanum_4.h"
+
+Mecanum_4::Mecanum_4(MD *md[NUM])
+{
+    wheel[LF] = new Wheel( md[LF],  1.0 * M_PI / 4.0,  3.0 * M_PI / 4.0 );
+    wheel[LB] = new Wheel( md[LB],  3.0 * M_PI / 4.0, -3.0 * M_PI / 4.0 );
+    wheel[RB] = new Wheel( md[RB], -3.0 * M_PI / 4.0, -1.0 * M_PI / 4.0 );
+    wheel[RF] = new Wheel( md[RF], -1.0 * M_PI / 4.0,  1.0 * M_PI / 4.0 );
+}
+
+Mecanum_4::Mecanum_4(MD *md_LF, MD *md_LB, MD *md_RB, MD *md_RF)
+{
+    wheel[LF] = new Wheel( md_LF,  1.0 * M_PI / 4.0,  3.0 * M_PI / 4.0 );
+    wheel[LB] = new Wheel( md_LB,  3.0 * M_PI / 4.0, -3.0 * M_PI / 4.0 );
+    wheel[RB] = new Wheel( md_RB, -3.0 * M_PI / 4.0, -1.0 * M_PI / 4.0 );
+    wheel[RF] = new Wheel( md_RF, -1.0 * M_PI / 4.0,  1.0 * M_PI / 4.0 );
+}
+
+Mecanum_4::Mecanum_4(Wheel *wheel[NUM])
+{
+    for(int i = 0; i < NUM; i++){
+        this->wheel[i] = wheel[i];
+    }
+}
+
+void Mecanum_4::move(double x, double y, double yaw)
+{
+    double move_angle, move_radius;
+    double duty[NUM];
+    double max_duty, limit_duty = 1.0;
+    
+    move_angle = atan2(y, x);
+    move_radius = sqrt( y * y + x * x );
+    
+    for(int i = 0; i < NUM; i++)
+        duty[i] = wheel[i]->wheel_speed(move_angle, move_radius, yaw);
+
+    // 平行移動と回転のうち、大きい方をmax_dutyに格納
+    if(move_radius > fabs(yaw))
+    	max_duty = move_radius;
+    else
+    	max_duty = fabs(yaw);
+
+	up_limit_balance(duty, NUM, max_duty);
+	down_limit_balance(duty, NUM, limit_duty);
+    
+    for(int i = 0; i < NUM; i++){
+        wheel[i]->drive(duty[i]);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Mecanum_4/Mecanum_4.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,33 @@
+
+#ifndef MECANUM_4_H_
+#define MECANUM_4_H_
+
+#include <shared/Movement/Movement.h>
+#include <shared/Wheel/Wheel.h>
+
+class Mecanum_4 : public Movement
+{
+public:
+    enum
+    {
+        LF,
+        LB,
+        RB,
+        RF,
+        NUM
+    };
+    
+    Mecanum_4(MD *md[NUM]);
+    Mecanum_4(MD *md_LF, MD *md_LB, MD *md_RB, MD *md_RF);
+    Mecanum_4(Wheel *wheel[NUM]);
+    
+    virtual void move(double x, double y, double yaw);
+    
+    
+private:
+    Wheel *wheel[NUM];
+
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Movement/Movement.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,37 @@
+#include "Movement.h"
+
+Movement::Movement()
+{}
+
+void Movement::up_limit_balance(double duty[], double units_num, double limit)
+{
+	double max_duty = 0;
+	// max_dutyにduty[i]の最大値を格納
+	for(int i = 0; i < units_num; i++){
+		if(fabs(duty[i]) > max_duty)
+	    	max_duty = fabs(duty[i]);
+	}
+
+	// 各車輪の比率を保ちながらduty[i]の最大値をlimitに引き上げ
+	if( max_duty != 0 && ( max_duty < limit ) ){
+	   	for(int i = 0; i < units_num; i++)
+	   		duty[i] *= limit / max_duty;
+	}
+}
+
+void Movement::down_limit_balance(double duty[], double units_num, double limit)
+{
+    // 各車輪の比率を保ちながらduty[i]を上限以下に引き下げ
+    for(int i = 0; i < units_num; i ++){
+        if(fabs(duty[i]) > limit){
+            double inv = fabs(duty[i]);
+            for(int j = 0; j < units_num; j ++)
+                duty[j] *= limit / inv;
+        }
+    }
+}
+
+void Movement::move(Eigen::Vector3d move_vector)
+{
+	move(move_vector(X), move_vector(Y), move_vector(Yaw));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Movement/Movement.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,34 @@
+#ifndef MOVEMENT_H_
+#define MOVEMENT_H_
+
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+
+#include <mbed.h>
+#include <Dense.h>
+
+class Movement
+{
+public:
+	Movement();
+	virtual ~Movement(){};
+
+	// 各ホイールの出力の比率を保ちながら、limitまで底上げ
+	void up_limit_balance(double duty[], double units_num, double limit);
+
+	// 各ホイールの出力の比率を保ちながら、limitまで抑制
+	void down_limit_balance(double duty[], double units_num, double limit);
+
+	enum VectorElements{
+		X,
+		Y,
+		Yaw
+	};
+
+	virtual void move(double x, double y, double yaw) = 0;
+	virtual void move(Eigen::Vector3d move_vector);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry2/Odometry2.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,71 @@
+#include "Odometry2.h"
+
+Odometry2::Odometry2(OdometryUnit *wheel[NUM]){
+	for (int i = 0; i < NUM; i++)
+		_wheel[i] = wheel[i];
+	reset_position();
+	_set_coefficient_matrix();
+}
+
+Eigen::Vector3d Odometry2::read_position() {
+
+	 Eigen::Vector3d velocity = _read_velocity();
+
+	 EigenCalculation::yawling_correct(&velocity);
+	 EigenCalculation::rotate_coordinate(&velocity, _position(Odometry2::Yaw));
+
+	 _position += velocity;
+	 return _position;
+}
+
+void Odometry2::write_position(const Eigen::Vector3d& set_position) {
+	_position = set_position;
+}
+
+void Odometry2::reset_position() {
+	_position = Eigen::Vector3d::Zero();
+}
+
+Eigen::Vector3d Odometry2::get_velocity() {
+	return _velocity;
+}
+
+Eigen::Vector3d Odometry2::_read_velocity() {
+	Eigen::Vector3d velocity;
+	Eigen::Vector2d wheel_velocity;
+
+	for (int i = 0; i < NUM; i++)
+		wheel_velocity(i) = _wheel[i]->read_wheel_velocity();
+
+	velocity = _coefficient_matrix * wheel_velocity;
+	_velocity = velocity;
+
+	return velocity;
+}
+
+void Odometry2::_set_coefficient_matrix() {
+	Matrix23d coefficient_matrix;
+	Matrix32d invert_coefficient_matrix;
+	for (int i = 0; i < NUM; i++)
+		_coefficient[i] = _wheel[i]->get_coefficient_elements();
+
+	for (int i = 0; i < NUM; i++) {
+		for (int j = 0; j < 3; j++) {
+			coefficient_matrix(i, j) = _coefficient[i](j);
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < NUM; j++) {
+			if (i == 2)
+				invert_coefficient_matrix(i, j) = 1.0
+						/ (2.0 * _coefficient[j](i));
+			else
+				invert_coefficient_matrix(i, j) = _coefficient[j](i);
+		}
+	}
+	_coefficient_matrix = invert_coefficient_matrix / 2.0;
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry2/Odometry2.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,48 @@
+
+#ifndef ODOMETRY4_H_
+#define ODOMETRY4_H_
+
+#include <shared/OdometryUnit/OdometryUnit.h>
+#include <Dense.h>
+#include <LU.h>
+#include <shared/EigenCalculation/EigenCalculation.h>
+
+typedef Eigen::Matrix<double, 2, 3> Matrix23d;
+typedef Eigen::Matrix<double, 3, 2> Matrix32d;
+
+class Odometry2
+{
+public:
+	static const int NUM = 2;
+	Odometry2(OdometryUnit *wheel[NUM]);
+	virtual ~Odometry2(){};
+
+	Eigen::Vector3d read_position();
+
+	void write_position(const Eigen::Vector3d& set_position);
+	void reset_position();
+
+	Eigen::Vector3d get_velocity();
+
+	void set_coefficient_matrix();
+
+	enum VectorElements{
+		X,
+		Y,
+		Yaw
+	};
+
+private:
+	Eigen::Vector3d _read_velocity();
+	void _set_coefficient_matrix();
+
+	OdometryUnit *_wheel[NUM];
+	Eigen::Vector3d _coefficient[NUM];
+	Matrix32d _coefficient_matrix;
+	Eigen::Vector3d _position;
+	Eigen::Vector3d _velocity;
+};
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry3/Odometry3.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,58 @@
+#include "Odometry3.h"
+
+Odometry3::Odometry3(OdometryUnit *wheel[3])
+{
+	for(int i = 0; i < NUM; i++)
+		_wheel[i] = wheel[i];
+	reset_position();
+	_set_coefficient_matrix();
+}
+
+Eigen::Vector3d Odometry3::read_position()
+{
+	Eigen::Vector3d velocity = _read_velocity();
+
+	EigenCalculation::yawling_correct(&velocity);
+	EigenCalculation::rotate_coordinate(&velocity, _position(Odometry3::Yaw));
+
+	_position += velocity;
+	return _position;
+}
+
+void Odometry3::write_position(const Eigen::Vector3d& set_position)
+{
+	_position = set_position;
+}
+
+void Odometry3::reset_position()
+{
+	_position = Eigen::Vector3d::Zero();
+}
+
+Eigen::Vector3d Odometry3::get_velocity()
+{
+	return _velocity;
+}
+
+Eigen::Vector3d Odometry3::_read_velocity()
+{
+	Eigen::Vector3d velocity;
+	Eigen::Vector3d wheel_velocity;
+
+	for(int i =0; i < NUM; i++)
+		wheel_velocity(i) = _wheel[i]->read_wheel_velocity();
+
+	velocity = _coefficient_matrix * wheel_velocity;
+
+	_velocity = velocity;
+
+	return velocity;
+}
+
+void Odometry3::_set_coefficient_matrix()
+{
+	for(int i = 0; i < NUM; i++)
+		_coefficient_matrix.row(i) = _wheel[i]->get_coefficient_elements();
+
+	EigenCalculation::invert_matrix(&_coefficient_matrix);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry3/Odometry3.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,40 @@
+#ifndef ODOMETRY3_H_
+#define ODOMETRY3_H_
+
+#include <shared/OdometryUnit/OdometryUnit.h>
+#include <Dense.h>
+#include <shared/EigenCalculation/EigenCalculation.h>
+
+class Odometry3
+{
+public:
+	static const int NUM = 3;
+	Odometry3(OdometryUnit *wheel[NUM]);
+	virtual ~Odometry3(){};
+
+	Eigen::Vector3d read_position();
+
+	void write_position(const Eigen::Vector3d& set_position);
+	void reset_position();
+
+	Eigen::Vector3d get_velocity();
+
+	void set_coefficient_matrix();
+
+	enum VectorElements{
+		X,
+		Y,
+		Yaw
+	};
+
+private:
+	Eigen::Vector3d _read_velocity();
+	void _set_coefficient_matrix();
+
+	OdometryUnit *_wheel[NUM];
+	Eigen::Matrix3d _coefficient_matrix;
+	Eigen::Vector3d _position;
+	Eigen::Vector3d _velocity;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry4/Odometry4.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,71 @@
+#include "Odometry4.h"
+
+Odometry4::Odometry4(OdometryUnit *wheel[NUM]){
+	for (int i = 0; i < NUM; i++)
+		_wheel[i] = wheel[i];
+	reset_position();
+	_set_coefficient_matrix();
+}
+
+Eigen::Vector3d Odometry4::read_position() {
+
+	 Eigen::Vector3d velocity = _read_velocity();
+
+	 EigenCalculation::yawling_correct(&velocity);
+	 EigenCalculation::rotate_coordinate(&velocity, _position(Odometry4::Yaw));
+
+	 _position += velocity;
+	 return _position;
+}
+
+void Odometry4::write_position(const Eigen::Vector3d& set_position) {
+	_position = set_position;
+}
+
+void Odometry4::reset_position() {
+	_position = Eigen::Vector3d::Zero();
+}
+
+Eigen::Vector3d Odometry4::get_velocity() {
+	return _velocity;
+}
+
+Eigen::Vector3d Odometry4::_read_velocity() {
+	Eigen::Vector3d velocity;
+	Eigen::Vector4d wheel_velocity;
+
+	for (int i = 0; i < NUM; i++)
+		wheel_velocity(i) = _wheel[i]->read_wheel_velocity();
+
+	velocity = _coefficient_matrix * wheel_velocity;
+	_velocity = velocity;
+
+	return velocity;
+}
+
+void Odometry4::_set_coefficient_matrix() {
+	Matrix43d coefficient_matrix;
+	Matrix34d invert_coefficient_matrix;
+	for (int i = 0; i < NUM; i++)
+		_coefficient[i] = _wheel[i]->get_coefficient_elements();
+
+	for (int i = 0; i < NUM; i++) {
+		for (int j = 0; j < 3; j++) {
+			coefficient_matrix(i, j) = _coefficient[i](j);
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < NUM; j++) {
+			if (i == 2)
+				invert_coefficient_matrix(i, j) = 1.0
+						/ (2.0 * _coefficient[j](i));
+			else
+				invert_coefficient_matrix(i, j) = _coefficient[j](i);
+		}
+	}
+	_coefficient_matrix = invert_coefficient_matrix / 2.0;
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry4/Odometry4.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,48 @@
+
+#ifndef ODOMETRY4_H_
+#define ODOMETRY4_H_
+
+#include <shared/OdometryUnit/OdometryUnit.h>
+#include <Dense.h>
+#include <LU.h>
+#include <shared/EigenCalculation/EigenCalculation.h>
+
+typedef Eigen::Matrix<double, 4, 3> Matrix43d;
+typedef Eigen::Matrix<double, 3, 4> Matrix34d;
+
+class Odometry4
+{
+public:
+	static const int NUM = 4;
+	Odometry4(OdometryUnit *wheel[NUM]);
+	virtual ~Odometry4(){};
+
+	Eigen::Vector3d read_position();
+
+	void write_position(const Eigen::Vector3d& set_position);
+	void reset_position();
+
+	Eigen::Vector3d get_velocity();
+
+	void set_coefficient_matrix();
+
+	enum VectorElements{
+		X,
+		Y,
+		Yaw
+	};
+
+private:
+	Eigen::Vector3d _read_velocity();
+	void _set_coefficient_matrix();
+
+	OdometryUnit *_wheel[NUM];
+	Eigen::Vector3d _coefficient[NUM];
+	Matrix34d _coefficient_matrix;
+	Eigen::Vector3d _position;
+	Eigen::Vector3d _velocity;
+};
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OdometryUnit/OdometryUnit.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,21 @@
+#ifndef ODOMETRY_UNIT_H
+#define ODOMETRY_UNIT_H
+
+#define EIGEN_NO_DEBUG // コード内のassertを無効化．
+#include <Core.h>
+
+class OdometryUnit
+{
+public:
+    virtual Eigen::Vector3d get_coefficient_elements() = 0;
+    virtual double read_wheel_velocity() = 0;
+
+    enum CoefficientElements 
+    {
+        X,
+        Y,
+        Yaw
+    };
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OdometryWheel/OdometryWheel.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,27 @@
+#include "OdometryWheel.h"
+
+OdometryWheel::OdometryWheel(QEI *qei, double wheel_angle, double wheel_radius, double position_angle, double position_radius)
+	:_qei(qei), _wheel_angle(wheel_angle), _wheel_radius(wheel_radius), _position_angle(position_angle), _position_radius(position_radius)
+{
+	_calculate_coefficient_elements();
+}
+
+Eigen::Vector3d OdometryWheel::get_coefficient_elements()
+{
+	return _coefficient_elements;
+}
+
+double OdometryWheel::read_wheel_velocity()
+{
+	double velocity;
+	velocity = _qei->get_ang() * _wheel_radius;
+	_qei->reset();
+	return velocity;
+}
+
+void OdometryWheel::_calculate_coefficient_elements()
+{
+	_coefficient_elements(X) = cos(_wheel_angle);
+	_coefficient_elements(Y) = sin(_wheel_angle);
+	_coefficient_elements(Yaw) = sin(_wheel_angle - _position_angle) * (_position_radius);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OdometryWheel/OdometryWheel.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,41 @@
+#ifndef ODMETRYWHEEL_H_
+#define ODMETRYWHEEL_H_
+
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+
+#include <shared/OdometryUnit/OdometryUnit.h>
+#include <shared/QEI/QEI.h>
+
+#define EIGEN_NO_DEBUG // コード内のassertを無効化．
+#include <Core.h>
+
+#define M_PI 3.14159265358979323846
+
+class OdometryWheel : public OdometryUnit
+{
+public:
+	OdometryWheel(QEI *qei, double wheel_angle, double wheel_radius, double position_angle, double position_radius);
+	virtual ~OdometryWheel(){};
+
+	virtual Eigen::Vector3d get_coefficient_elements();
+	virtual double read_wheel_velocity();
+
+	enum CoefficientElements{
+		X,
+		Y,
+		Yaw
+	};
+
+private:
+	void _calculate_coefficient_elements();
+
+	QEI *_qei;
+	Eigen::Vector3d _coefficient_elements;
+
+	double _wheel_angle, _wheel_radius, _position_angle, _position_radius;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Omni_3/Omni_3.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,47 @@
+#include "Omni_3.h"
+
+Omni_3::Omni_3(MD *md[NUM])
+{
+    _wheel[F] = new Wheel( md[F],  3.0 * M_PI / 3.0,  3.0 * M_PI / 6.0 );
+    _wheel[L] = new Wheel( md[L],  -1.0 * M_PI / 3.0, -5.0 * M_PI / 6.0 );
+    _wheel[R] = new Wheel( md[R],  1.0 * M_PI / 3.0, -1.0 * M_PI / 6.0 );
+}
+
+Omni_3::Omni_3(MD *md_F, MD *md_L, MD *md_R)
+{
+    _wheel[F] = new Wheel( md_F,  0.0 * M_PI / 3.0,  3.0 * M_PI / 6.0 );
+    _wheel[L] = new Wheel( md_L,  2.0 * M_PI / 3.0, -5.0 * M_PI / 6.0 );
+    _wheel[R] = new Wheel( md_R, -2.0 * M_PI / 3.0, -1.0 * M_PI / 6.0 );
+}
+
+Omni_3::Omni_3(Wheel *wheel[NUM])
+{
+	for(int i = 0; i < NUM; i++)
+		_wheel[i] = wheel[i];
+}
+
+void Omni_3::move(double x, double y, double yaw)
+{
+    double move_angle, move_radius;
+    double duty[NUM];
+    double max_duty, limit_duty = 1.0;
+    
+    move_angle = atan2(y, x);
+    move_radius = sqrt( y * y + x * x );
+    
+    for(int i = 0; i < NUM; i++)
+        duty[i] = _wheel[i]->wheel_speed(move_angle, move_radius, yaw);
+
+    // 蟷ｳ陦檎ｧｻ蜍輔→蝗櫁ｻ｢縺ｮ縺�縺｡縲∝､ｧ縺阪＞譁ｹ繧知ax_duty縺ｫ譬ｼ邏�
+    if(move_radius > fabs(yaw))
+    	max_duty = move_radius;
+    else
+    	max_duty = fabs(yaw);
+
+	up_limit_balance(duty, NUM, max_duty);
+	down_limit_balance(duty, NUM, limit_duty);
+    
+    for(int i = 0; i < NUM; i++){
+        _wheel[i]->drive(duty[i]);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Omni_3/Omni_3.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,32 @@
+#ifndef OMNI_3_H_
+#define OMNI_3_H_
+
+#include <shared/Movement/Movement.h>
+#include <shared/Wheel/Wheel.h>
+
+class Omni_3 : public Movement
+{
+public:
+    enum
+    {
+        F,
+        L,
+        R,
+        NUM
+    };
+    
+    Omni_3(MD *md[NUM]);
+    Omni_3(MD *md_F, MD *md_L, MD *md_R);
+    Omni_3(Wheel *wheel[NUM]);
+    virtual ~Omni_3(){};
+    
+    virtual void move(double x, double y, double yaw);
+    
+    
+private:
+    Wheel *_wheel[NUM];
+
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OnOffMovement/OnOffMovement.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,35 @@
+#include "OnOffMovement.h"
+
+OnOffMovement::OnOffMovement(Movement *movement) : AutomaticMovement(movement)
+{}
+
+Eigen::Vector3d OnOffMovement::move_automatic(Eigen::Vector3d current_position)
+{
+	Eigen::Vector3d position_difference, speed_move;
+	position_difference = _target_position - current_position;
+
+	for(int i = 0; i < 3; i++){
+		if(fabs(position_difference(i)) < _near_distance(i))
+			speed_move(i) = 0;
+		else
+			speed_move(i) = position_difference(i)
+							/ fabs(position_difference(i))
+							* _speed(i);
+	}
+
+	EigenCalculation::rotate_coordinate(&speed_move, -current_position(Movement::Yaw));
+
+	_movement->move(speed_move);
+
+	return speed_move;
+}
+
+void OnOffMovement::set_near_distance(Eigen::Vector3d near_distance)
+{
+	_near_distance = near_distance;
+}
+
+void OnOffMovement::set_speed(Eigen::Vector3d speed)
+{
+	_speed = speed;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OnOffMovement/OnOffMovement.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,23 @@
+#ifndef ONOFFMOVEMENT_H_
+#define ONOFFMOVEMENT_H_
+
+#include <shared/AutomaticMovement/AutomaticMovement.h>
+
+class OnOffMovement : public AutomaticMovement
+{
+public:
+	OnOffMovement(Movement *movement);
+	virtual ~OnOffMovement(){}
+
+	virtual Eigen::Vector3d move_automatic(Eigen::Vector3d current_position);
+
+	void set_near_distance(Eigen::Vector3d near_distance);
+	void set_speed(Eigen::Vector3d speed);
+
+private:
+
+	Eigen::Vector3d _near_distance;
+	Eigen::Vector3d _speed;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/I_PD/I_PD.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,28 @@
+#include "I_PD.h"
+
+double I_PD::_get_control()
+{
+    return PID::_pid_state.k * (_i - (_p +_d));
+}
+void I_PD::_calc_p()
+{
+    _update(_diff, PID::_pid_state.target - PID::_pid_state.current);
+    _update(_measure, PID::_pid_state.target);
+    _p = _measure[1];
+}
+void I_PD::_calc_i()
+{
+    _integral += (_diff[0] + _diff[1]) / 2;
+    _i = _integral;
+}
+void I_PD::_calc_d()
+{
+    _d = _measure[1] - _measure[0];
+}
+void I_PD::_reset()
+{
+    _p = _i = _d = 0;
+    _diff[0] = _diff[1] = 0;
+    _integral = 0;
+    _measure[0] = _measure[1] = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/I_PD/I_PD.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,37 @@
+#ifndef I_PD_H_
+#define I_PD_H_
+
+#include "PID.h"
+
+class I_PD : public PID
+{
+public:
+    I_PD(double k = 0, double Ti = 0, double Td = 0, double target = 0, double current = 0)
+    {
+        PID::set_PID(k, Ti, Td);
+        set_state(target, current);
+    }
+    ~I_PD(){}
+    
+protected: 
+    
+    virtual double _get_control();
+    virtual void _calc_p();
+    virtual void _calc_i();
+    virtual void _calc_d();
+    virtual void _reset();
+
+private:
+
+    inline void _update(double *array, double present)
+    {
+        array[0] = array[1];
+        array[1] = present;
+    }
+    double _p, _i, _d;
+    double _diff[2];
+    double _integral;
+    double _measure[2];
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PID.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,42 @@
+#include "PID.h"
+
+double PID::get_control()
+{
+    _calc_p();
+    return _get_control();
+}
+
+void PID::set_state(double target, double current)
+{
+    _pid_state.target = target;
+    _pid_state.current = current;
+}
+
+void PID::set_PID(double k, double Ti, double Td)
+{
+    _pid_state.k = k;
+    _pid_state.Ti = Ti;
+    _pid_state.Td = Td;
+    reset();
+}
+
+void PID::reset()
+{
+    _reset();
+    if(_pid_state.Ti != 0)
+        _tick_i.attach(callback(this, &PID::_on_isr_i), _pid_state.Ti);
+    if(_pid_state.Td != 0)
+        _tick_d.attach(callback(this, &PID::_on_isr_d), _pid_state.Td);
+}
+
+void PID::_on_isr_i()
+{
+    _calc_p();
+    _calc_i();
+}
+
+void PID::_on_isr_d()
+{
+    _calc_p();
+    _calc_d();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PID.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,45 @@
+#ifndef PID_H_
+#define PID_H_
+
+#include "mbed.h"
+
+class PID
+{
+public:
+    PID(): _pid_state()
+    {}
+    ~PID()
+    {
+        _tick_i.detach();
+        _tick_d.detach();
+    }
+    
+    double get_control();
+    void set_state(double target, double current);
+    
+    void set_PID(double k, double Ti, double Td);
+    void reset();
+protected: 
+    typedef struct pid_con_s{
+        double k;
+        double Ti;
+        double Td;
+        double current;
+        double target;
+    }pid_con_t;
+    
+    pid_con_t _pid_state;
+    
+    virtual double _get_control() = 0;
+    virtual void _calc_p() = 0;
+    virtual void _calc_i() = 0;
+    virtual void _calc_d() = 0;
+    virtual void _reset() = 0;
+private:
+    Ticker _tick_i, _tick_d;
+    
+    void _on_isr_i();
+    void _on_isr_d();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PI_D/PI_D.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,28 @@
+#include "PI_D.h"
+
+double PI_D::_get_control()
+{
+    return PID::_pid_state.k * (_p + _i - _d);
+}
+void PI_D::_calc_p()
+{
+    _update(_diff, PID::_pid_state.target - PID::_pid_state.current);
+    _update(_measure, PID::_pid_state.target);
+    _p = _diff[1];
+}
+void PI_D::_calc_i()
+{
+    _integral += (_diff[0] + _diff[1]) / 2;
+    _i = _integral;
+}
+void PI_D::_calc_d()
+{
+    _d = _measure[1] - _measure[0];
+}
+void PI_D::_reset()
+{
+    _p = _i = _d = 0;
+    _diff[0] = _diff[1] = 0;
+    _integral = 0;
+    _measure[0] = _measure[1] = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PI_D/PI_D.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,37 @@
+#ifndef PI_D_H_
+#define PI_D_H_
+
+#include "PID.h"
+
+class PI_D : public PID
+{
+public:
+    PI_D(double k = 0, double Ti = 0, double Td = 0, double target = 0, double current = 0)
+    {
+        PID::set_PID(k, Ti, Td);
+        set_state(target, current);
+    }
+    ~PI_D(){}
+    
+protected: 
+    
+    virtual double _get_control();
+    virtual void _calc_p();
+    virtual void _calc_i();
+    virtual void _calc_d();
+    virtual void _reset();
+
+private:
+
+    inline void _update(double *array, double present)
+    {
+        array[0] = array[1];
+        array[1] = present;
+    }
+    double _p, _i, _d;
+    double _diff[2];
+    double _integral;
+    double _measure[2];
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PPID/PPID.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,26 @@
+#include "PPID.h"
+
+double PPID::_get_control()
+{
+    return PID::_pid_state.k * (_p + _i + _d);
+}
+void PPID::_calc_p()
+{
+    _update(_diff, PID::_pid_state.target - PID::_pid_state.current);
+    _p = _diff[1];
+}
+void PPID::_calc_i()
+{
+    _integral += (_diff[0] + _diff[1]) / 2;
+    _i = _integral;
+}
+void PPID::_calc_d()
+{
+    _d = _diff[1] - _diff[0];
+}
+void PPID::_reset()
+{
+    _p = _i = _d = 0;
+    _diff[0] = _diff[1] = 0;
+    _integral = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/PPID/PPID.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,36 @@
+#ifndef PPID_H_
+#define PPID_H_
+
+#include "PID.h"
+
+class PPID : public PID
+{
+public:
+    PPID(double k = 0, double Ti = 0, double Td = 0, double target = 0, double current = 0)
+    {
+        PID::set_PID(k, Ti, Td);
+        set_state(target, current);
+    }
+    ~PPID(){}
+    
+protected: 
+    
+    virtual double _get_control();
+    virtual void _calc_p();
+    virtual void _calc_i();
+    virtual void _calc_d();
+    virtual void _reset();
+
+private:
+
+    inline void _update(double *array, double present)
+    {
+        array[0] = array[1];
+        array[1] = present;
+    }
+    double _p, _i, _d;
+    double _diff[2];
+    double _integral;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/VPID/VPID.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,30 @@
+#include "VPID.h"
+
+double VPID::_get_control()
+{
+    return _ctrl;
+}
+void VPID::_calc_p()
+{
+    _update(_diff, PID::_pid_state.target - PID::_pid_state.current);
+    _p = _diff[1];
+    _update(_dctrl, PID::_pid_state.k * (_p + _i + _d));
+    _ctrl += (_dctrl[1] - _dctrl[0]) / _tim.read();
+    _tim.reset();
+}
+void VPID::_calc_i()
+{
+    _integral += (_diff[0] + _diff[1]) / 2;
+    _i = _integral;
+}
+void VPID::_calc_d()
+{
+    _d = _diff[1] - _diff[0];
+}
+void VPID::_reset()
+{
+    _p = _i = _d = 0;
+    _diff[0] = _diff[1] = 0;
+    _integral = 0;
+    _ctrl = _dctrl[0] = _dctrl[1] = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/PID_Control/VPID/VPID.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,39 @@
+#ifndef VPID_H_
+#define VPID_H_
+
+#include "PID.h"
+
+class VPID : public PID
+{
+public:
+    VPID(double k = 0, double Ti = 0, double Td = 0, double target = 0, double current = 0)
+    {
+        PID::set_PID(k, Ti, Td);
+        set_state(target, current);
+    }
+    ~VPID(){}
+    
+protected: 
+    
+    virtual double _get_control();
+    virtual void _calc_p();
+    virtual void _calc_i();
+    virtual void _calc_d();
+    virtual void _reset();
+
+private:
+    Timer _tim;
+
+    inline void _update(double *array, double present)
+    {
+        array[0] = array[1];
+        array[1] = present;
+    }
+    double _p, _i, _d;
+    double _diff[2];
+    double _dctrl[2];
+    double _ctrl;
+    double _integral;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/QEI/QEI.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,29 @@
+#include "QEI.h"
+
+QEI::QEI(int edge_per_revolution)
+	:_edge_per_revolution(edge_per_revolution)
+{}
+
+int QEI::read()
+{
+	return _read();
+}
+
+void QEI::write(int32_t count)
+{
+	_write(count);
+}
+
+void QEI::reset()
+{
+	_write(0);
+}
+
+double QEI::get_ang()
+{
+	return ((double)read() / (double)_edge_per_revolution) * (2 * M_PI); //[rad]
+}
+
+double QEI::get_ang_vel(){
+	return (_get_pps() / (double)_edge_per_revolution) * (2 * M_PI); //[rad/s]
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/QEI/QEI.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,31 @@
+#ifndef QEI_H_
+#define QEI_H_
+
+#include <mbed.h>
+
+#define M_PI 3.14159265358979323846
+
+class QEI
+{
+public:
+	QEI(int edge_per_revolution);
+	virtual ~QEI(){};
+	
+	int read();
+	
+	double get_ang();
+	double get_ang_vel();
+	
+	void write(int32_t);
+	void reset();
+	
+protected:
+    virtual int32_t _read() = 0;
+    virtual void _write(int32_t set_count) = 0;
+    virtual double _get_pps() = 0; 
+
+private:
+    int _edge_per_revolution;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Wheel/Wheel.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,48 @@
+#include "Wheel.h"
+
+Wheel::Wheel(MD *md, double wheel_angle, double position_angle)
+{
+    this->md = md;
+    this->wheel_angle = wheel_angle;
+    this->wheel_radius = 0;
+    
+    this->position_angle = position_angle;
+    this->position_radius = 1;
+}
+
+Wheel::Wheel(MD *md, double wheel_angle, double position_x, double position_y, double wheel_radius)
+{
+    this->md = md;
+    this->wheel_angle = wheel_angle;
+    this->wheel_radius = wheel_radius;
+    
+    set_position(position_x, position_y);
+}
+    
+void Wheel::set_position(double position_x, double position_y)
+{
+    position_angle = atan2(position_y, position_x);
+    position_radius = sqrt( position_y * position_y + position_x * position_x );
+}
+    
+double Wheel::wheel_speed(double move_angle, double move_radius, double yaw)
+{
+    double speed;
+    double moving, rotating;
+    
+    while(move_angle > M_PI)
+        move_angle -= 2.0 * M_PI;
+    
+    moving = cos(wheel_angle - move_angle) * move_radius;
+    
+    rotating = sin(wheel_angle - position_angle) * yaw * position_radius;
+    
+    speed = moving + rotating;
+    
+    return speed;
+}
+
+void Wheel::drive(double duty)
+{
+    md->drive(duty);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Wheel/Wheel.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,30 @@
+
+#ifndef WHEEL_H_
+#define WHEEL_H_
+
+#include <mbed.h>
+#include <shared/MD/MD.h>
+
+class Wheel
+{
+public:
+    Wheel(MD *md, double wheel_angle, double position_angle);
+    Wheel(MD *md, double wheel_angle, double position_x, double position_y, double wheel_radius = 0);
+    
+    void set_position(double position_x, double position_y);
+    
+    double wheel_speed(double move_angle, double move_radius, double yaw);
+    
+    void drive(double duty);
+private:
+    MD *md;
+
+    double position_angle, position_radius;
+    double wheel_angle, wheel_radius;
+};
+    
+#endif
+
+#ifndef M_PI
+#define M_PI 3.1415926535
+#endif