Revision 0:9a1bde373ef0, committed 2022-01-26

Comitter:

e5119053f6

Date:

Wed Jan 26 11:22:14 2022 +0000

Child:

1:aee399b81802

Commit message:

added the "DDSScontrol"

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,332 @@
+#include "Controller.h"
+
+Controller::Controller(PinName tx, PinName rx, int baudrate) : serial(tx, rx, baudrate)
+{
+    conData.ButtonState = 0;
+    conData.RJoyX = 127, conData.RJoyY = 127, conData.LJoyX = 127, conData.LJoyY = 127;
+
+    lastButtonState = 0;
+
+    comCheck = false;
+    conAvailable = false;
+    time_out_ms = -1;
+}
+
+void Controller::init(int _time_out_ms, int _int_time_ms)
+{
+    time_out_ms = _time_out_ms;
+    int_time_ms = _int_time_ms;
+    //serial.attach(this, &Controller::update, Serial::RxIrq);
+    //timer.start();
+}
+
+bool Controller::update()
+{
+    //receptionTime = timer.read();
+    static int count_ms = time_out_ms, pre_count_ms = 0; //受信時刻と前回の受信時刻
+    count_ms += int_time_ms;
+
+    char receive_data[10];
+    unsigned int loop_count = 0, checksum = 0x00;
+    comCheck = false;
+
+#if CON_TYPE == CON_ADACHI // 安達君開発のコントローラを使う場合の処理(どのコントローラを使うかはdefine.hで設定)
+    while (loop_count < 10 && serial.readable())
+    {
+        if (serial_recieve() == '\n')
+        {
+            for (int i = 0; i < 8; i++)
+                receive_data[i] = serial_recieve();
+            for (int i = 0; i < 8; i++)
+                receive_data[i] -= 0x20;
+            for (int i = 0; i < 7; i++)
+                checksum ^= receive_data[i];
+
+            if (receive_data[7] == checksum & 0xFF)
+            {
+                comCheck = true;
+
+                //pre_conData.ButtonState = conData.ButtonState;
+                lastButtonState = ((receive_data[0] & 0x3F) << 2) | ((receive_data[1] & 0x30) >> 4);
+                conData.ButtonState |= lastButtonState;
+
+                conData.RJoyX = ((receive_data[1] & 0x0F) << 4) | ((receive_data[2] & 0x3C) >> 2);
+                conData.RJoyY = ((receive_data[2] & 0x03) << 6) | (receive_data[3] & 0x3F);
+                conData.LJoyX = ((receive_data[4] & 0x3F) << 2) | ((receive_data[5] & 0x30) >> 4);
+                conData.LJoyY = ((receive_data[5] & 0x0F) << 4) | (receive_data[6] & 0x0F);
+
+                break;
+            }
+            
+            pre_count_ms = count_ms; //受信時間の更新
+        }
+        loop_count++;
+    }
+#elif CON_TYPE == CON_ELECOM // ELECOMのコントローラを使う場合の処理(どのコントローラを使うかはdefine.hで設定)
+    // コントローラデータを取得する部分
+    static int recv_num = 0;
+    char c;
+    while (serial.readable())
+    {
+        c = serial.getc();
+        if (c == '\n')
+        {
+            if (recv_num == 10)
+            { // チェックサムは無く，9個受信したら値を格納
+                for (int i = 0; i < 9; i++)
+                    checksum += (unsigned int)(receive_data[i] - 0x20); // チェックサムの計算
+                if ((checksum & 0x3F) == (receive_data[9] - 0x20))
+                { // チェックサムの計算が合っていた場合のみ値を格納
+                    comCheck = true;
+
+                    //conData.ButtonState = 0;
+                    conData.LJoyX = 0, conData.LJoyY = 0, conData.RJoyX = 0, conData.RJoyY = 0;
+                    lastButtonState = (unsigned int)(receive_data[0] - 0x20);
+                    lastButtonState |= (unsigned int)(receive_data[1] - 0x20) << 6;
+                    lastButtonState |= (unsigned int)(receive_data[2] - 0x20) << 12;
+
+                    conData.LJoyX |= (unsigned int)(receive_data[3] - 0x20);
+                    conData.LJoyX |= (unsigned int)((receive_data[4] - 0x20) & 0x03) << 6;
+                    conData.LJoyX = abs(conData.LJoyX - 0xFF);
+
+                    conData.LJoyY |= (unsigned int)((receive_data[4] - 0x20) & 0x3C) >> 2;
+                    conData.LJoyY |= (unsigned int)((receive_data[5] - 0x20) & 0x0F) << 4;
+                    conData.LJoyY = abs(conData.LJoyY - 0xFF);
+
+                    conData.RJoyX |= (unsigned int)((receive_data[5] - 0x20) & 0x30) >> 4;
+                    conData.RJoyX |= (unsigned int)((receive_data[6] - 0x20) & 0x3F) << 2;
+                    conData.RJoyX = abs(conData.RJoyX - 0xFF);
+
+                    conData.RJoyY |= (unsigned int)(receive_data[7] - 0x20);
+                    conData.RJoyY |= (unsigned int)((receive_data[8] - 0x20) & 0x03) << 6;
+                    conData.RJoyY = abs(conData.RJoyY - 0xFF);
+
+                    int buttonPushNum = 0;
+                    for (int i = 0; i < 16; i++)
+                    {
+                        buttonPushNum += (conData.ButtonState >> i) & 0x0001;
+                    }
+                    if (buttonPushNum > 5)
+                    {
+                        //conData.ButtonState = pre_conData.ButtonState;
+                        comCheck = false;
+                    }
+                    else
+                    {
+                        conData.ButtonState |= lastButtonState;
+                    }
+                    
+                    pre_count_ms = count_ms; //受信時間の更新
+                }
+            }
+            recv_num = 0;
+        }
+        else
+        {
+            receive_data[recv_num] = c;
+            recv_num++;
+        }
+    }
+#elif CON_TYPE == CON_DS4    // DualShock4を使う場合の処理(どのコントローラを使うかはdefine.hで設定)
+    // コントローラデータを取得する部分
+    static int recv_num = 0;
+    char c;
+    while (serial.readable())
+    {
+        c = serial.getc();
+        //Serial.print(c);
+        if (c == '\n')
+        {
+            if (recv_num == 10)
+            { // データ数はチェックサム含めて10個(0~9)
+                checksum = 0;
+                for (int i = 0; i < 9; i++)
+                    checksum ^= (unsigned int)(receive_data[i] - 0x20); // チェックサムの計算
+                if ((checksum & 0x3F) == (receive_data[9] - 0x20))
+                { // チェックサムの計算が合っていた場合のみ値を格納
+                    comCheck = true;
+
+                    //conData.ButtonState = 0;
+                    conData.LJoyX = 0, conData.LJoyY = 0, conData.RJoyX = 0, conData.RJoyY = 0;
+                    lastButtonState = (unsigned int)(receive_data[0] - 0x20) & 0x3F;
+                    lastButtonState |= (unsigned int)((receive_data[1] - 0x20) & 0x3F) << 6;
+                    lastButtonState |= (unsigned int)((receive_data[2] - 0x20) & 0x0F) << 12;
+
+                    conData.LJoyX |= (unsigned int)(receive_data[3] - 0x20);
+                    conData.LJoyX |= (unsigned int)((receive_data[4] - 0x20) & 0x03) << 6;
+                    conData.LJoyX = abs(conData.LJoyX - 0xFF);
+
+                    conData.LJoyY |= (unsigned int)((receive_data[4] - 0x20) & 0x3C) >> 2;
+                    conData.LJoyY |= (unsigned int)((receive_data[5] - 0x20) & 0x0F) << 4;
+                    conData.LJoyY = abs(conData.LJoyY - 0xFF);
+
+                    conData.RJoyX |= (unsigned int)((receive_data[5] - 0x20) & 0x30) >> 4;
+                    conData.RJoyX |= (unsigned int)((receive_data[6] - 0x20) & 0x3F) << 2;
+                    conData.RJoyX = abs(conData.RJoyX - 0xFF);
+
+                    conData.RJoyY |= (unsigned int)(receive_data[7] - 0x20);
+                    conData.RJoyY |= (unsigned int)((receive_data[8] - 0x20) & 0x03) << 6;
+                    conData.RJoyY = abs(conData.RJoyY - 0xFF);
+
+                    // 通信ミスであり得ない数のボタン数押されていた場合に無視する処理
+                    int buttonPushNum = 0;
+                    for (int i = 0; i < 16; i++)
+                    {
+                        buttonPushNum += (lastButtonState >> i) & 0x0001;
+                    }
+                    if (buttonPushNum > 5)
+                    {
+                        //conData.ButtonState = pre_conData.ButtonState;
+                        comCheck = false;
+                    }
+                    else
+                    {
+                        conData.ButtonState = lastButtonState & 0xFFFF;
+                    }
+
+                    pre_count_ms = count_ms; //受信時間の更新
+                }
+            }
+            recv_num = 0;
+        }
+        else
+        {
+            receive_data[recv_num] = c;
+            recv_num++;
+        }
+    }
+
+#endif
+
+    if(!(time_out_ms == -1)) conAvailable = (time_out_ms > (count_ms - pre_count_ms)); //タイムアウトとインターバルの比較
+    else conAvailable = true;
+    if(count_ms > time_out_ms * 1000) count_ms = time_out_ms; //オーバーフロー対策
+
+    return comCheck;
+}
+
+bool Controller::getComCheck(void)
+{
+    return comCheck;
+}
+
+bool Controller::available(void)
+{
+    return conAvailable;
+}
+
+bool Controller::readButton_bin(unsigned int ButtonNum)
+{ //放しているときは０，押しているときは１
+    return ((conData.ButtonState & (0x0001 << (ButtonNum - 1))) == (0x0001 << (ButtonNum - 1))) ? true : false;
+}
+
+int Controller::readButton(unsigned int ButtonNum)
+{ //放しているときは０，押しているときは１，押した瞬間は２，放した瞬間は－１
+    int result = 0;
+    if ((conData.ButtonState & (0x0001 << (ButtonNum - 1))) == (0x0001 << (ButtonNum - 1)))
+        result += 2;
+    if ((pre_conData.ButtonState & (0x0001 << (ButtonNum - 1))) == (0x0001 << (ButtonNum - 1)))
+        result -= 1;
+    return result;
+}
+
+unsigned int Controller::getButtonState()
+{
+    return conData.ButtonState;
+}
+
+void Controller::clearButtonState()
+{
+    pre_conData.ButtonState = conData.ButtonState;
+}
+
+ControllerData Controller::getConData()
+{
+    return conData;
+}
+
+double Controller::readJoyRX()
+{
+    if (conData.RJoyX == 127)
+        return 0;
+    return ((double)conData.RJoyX - 127.5) / 127.5;
+}
+
+double Controller::readJoyRY()
+{
+    if (conData.RJoyY == 127)
+        return 0;
+    return ((double)conData.RJoyY - 127.5) / 127.5;
+}
+
+double Controller::readJoyLX()
+{
+    if (conData.LJoyX == 127)
+        return 0;
+    return ((double)conData.LJoyX - 127.5) / 127.5;
+}
+
+double Controller::readJoyLY()
+{
+    if (conData.LJoyY == 127)
+        return 0;
+    return ((double)conData.LJoyY - 127.5) / 127.5;
+}
+
+uint8_t Controller::readJoyRXbyte()
+{
+    return conData.RJoyX;
+}
+
+uint8_t Controller::readJoyRYbyte()
+{
+    return conData.RJoyY;
+}
+
+uint8_t Controller::readJoyLXbyte()
+{
+    return conData.LJoyX;
+}
+
+uint8_t Controller::readJoyLYbyte()
+{
+    return conData.LJoyY;
+}
+
+unsigned int Controller::getButtonFlagRise()
+{
+    // 立ち上がり，立下りのフラッギング処理 (フラグクリアは別関数で)
+    unsigned int buttonFlagRise = 0;
+    if (pre_conData.ButtonState != conData.ButtonState)
+    {
+        for (int i = 0; i < 16; i++)
+        {
+            int mask = 0x01 << i;
+            if ((conData.ButtonState & mask) != (pre_conData.ButtonState & mask))
+            {
+                if ((conData.ButtonState & mask) == mask)
+                    buttonFlagRise |= (conData.ButtonState & mask);
+            }
+        }
+    }
+    return buttonFlagRise;
+}
+
+unsigned int Controller::getButtonFlagFall()
+{
+    unsigned int buttonFlagFall = 0;
+    if (pre_conData.ButtonState != conData.ButtonState)
+    {
+        for (int i = 0; i < 16; i++)
+        {
+            int mask = 0x01 << i;
+            if ((conData.ButtonState & mask) != (pre_conData.ButtonState & mask))
+            {
+                if ((pre_conData.ButtonState & mask) == mask)
+                    buttonFlagFall |= (pre_conData.ButtonState & mask);
+            }
+        }
+    }
+    return buttonFlagFall;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,74 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include "mbed.h"
+#include "RawSerial.h"
+#include "define.h"
+
+struct ControllerData{
+    unsigned int ButtonState;
+    uint8_t RJoyX, RJoyY, LJoyX, LJoyY;
+};
+
+class Controller{
+    public:
+        Controller(PinName tx, PinName rx, int baudrate);
+        int count_ms, pre_count_ms; //受信時刻と前回の受信時刻
+
+        bool readButton_bin(unsigned int ButtonNum); //押していない時はfalse(0),押してるときはtrue(1)を返す．　ButtonNumはデータの欲しいボタンの名前を
+        int  readButton(unsigned int ButtonNum);     //上にプラスして 押した瞬間は2，放した瞬間は-1を返す．    define.hを参考に数字を入力しても良い
+        bool getComCheck(void); //値が更新されたときにtrueを返す．
+        bool update(); //受信の処理＋ボタンの情報の更新．受信割込みで処理
+        
+        void init(int _time_out_ms, int _int_time_ms); //コントローラの通信速度と通信のタイムアウト時間を設定
+        bool available(void);
+
+        unsigned int getButtonState();  //分解する前のButtonStateの情報をprint 0~255の値をとる
+        void clearButtonState();
+        ControllerData getConData();
+        unsigned int getButtonFlagRise();
+        unsigned int getButtonFlagFall();
+    
+                                //       X
+        double readJoyRX();     //       ^ 
+        double readJoyRY();     //       | 
+        double readJoyLX();     //  Y<---+----
+        double readJoyLY();     //       | 
+                                //       | 
+                                //  1.0  ~   -1.0
+
+                                   //       X
+        uint8_t readJoyRXbyte();   //       ^ 
+        uint8_t readJoyRYbyte();   //       |
+        uint8_t readJoyLXbyte();   //  Y<---+----
+        uint8_t readJoyLYbyte();   //       | 
+                                   //       |
+                                   //  255  ~    0
+
+    private:
+        
+        RawSerial serial;
+        //Timer timer;
+
+        bool comCheck;
+        ControllerData conData;
+        ControllerData pre_conData;
+        unsigned int lastButtonState;
+
+        int time_out_ms;
+        double int_time_ms;
+        bool conAvailable;
+
+        uint8_t serial_recieve(){
+            char temp;
+            do{
+                temp = serial.getc();
+            }
+            while(temp==-1);
+            //CONTROL.write(temp);    //受け取ったデータをTXピンからそのまま送っている．他のマイコンにも流したいとき用．
+            return temp;
+        }
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/define.h	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,256 @@
+#ifndef DEFINE_h
+#define DEFINE_h
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846
+#endif
+
+struct coords {
+    double x;
+    double y;
+    double z;
+};
+
+#define RED  0
+#define BLUE 1
+
+#define SERIAL_UPPER    Serial0
+#define SERIAL_LPMSME1  Serial1
+#define SERIAL_ROBOCLAW Serial4
+// #define SERIAL_CON      Serial5
+#define SERIAL_CON      Serial7 // XBeeからコントローラデータを受信する場合
+#define SERIAL_LCD      Serial6
+#define SERIAL_M5STACK  Serial6
+//#define SERIAL_XBEE     Serial7
+
+#define PIN_XBEERESET 66
+
+// スイッチやLEDのピン設定
+#define PIN_DIP1 25
+#define PIN_DIP2 24
+#define PIN_DIP3 69
+#define PIN_DIP4 70
+
+#define PIN_SW_UP    32
+#define PIN_SW_LEFT  33
+#define PIN_SW_RIGHT 31
+#define PIN_SW_DOWN  30
+
+#define PIN_SW_B  29
+#define PIN_SW_A  28
+
+#define PIN_ENC_A  26
+#define PIN_ENC_B  27
+
+#define PIN_LED_1 20
+#define PIN_LED_2 36
+#define PIN_LED_3 37
+#define PIN_LED_4 38
+#define PIN_LED_ENC 40
+
+// 制御周期
+#define INT_TIME			( 0.01 )//( 0.001 )
+
+// >>> AutoControlで使用 >>>>>>>>>>>>>>>>>>
+// フェーズ管理
+#define STATE0  ( 7 )
+#define STATE1  ( 8 )
+#define STATE2  ( 9 )
+// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+// >>> ManualControlで使用 >>>>>>>>>>>>>>>>>>
+#define JOY_DEADBAND    ( 2 )
+#define JOY_MAXVEL      ( 0.5 )
+#define JOY_MAXANGVEL   ( 2.5 )
+#define MANUAL_LOWPASS_T  ( 0.01 )
+// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+// >>> PathTrackingで使用 >>>>>>>>>>>>>>>>>>
+#define POSI_X_KP    ( 2.5 )
+#define POSI_X_KI    ( 0.0 )
+#define POSI_X_KD    ( 5.0 )
+
+#define POSI_Y_KP    ( 3.0 )
+#define POSI_Y_KI    ( 0.0 )
+#define POSI_Y_KD    ( 2.0 )
+
+#define POSI_Z_KP    ( 4.0 )
+#define POSI_Z_KI    ( 0.0 )
+#define POSI_Z_KD    ( 0.0 )
+
+#define YOKOZURE_KP    ( 3.0 )
+#define YOKOZURE_KI    ( 0.0 )
+#define YOKOZURE_KD    ( 1.5 )
+
+#define KAKUDO_KP    ( 4.0 )
+#define KAKUDO_KI    ( 0.0 )
+#define KAKUDO_KD    ( 0.0 )
+
+#define FILT_SOKUDO_OMEGA ( 22.0 )
+#define FILT_SOKUDO_DZETA ( 1.0 )
+
+#define FILT_KAKUDO_OMEGA ( 10.0 )
+#define FILT_KAKUDO_DZETA ( 1.0 )
+// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+// >>> Platformで使用 >>>>>>>>>>>>>>>>>>>>>
+// Platformの種類を設定
+#define PLATFORM_MECHANUM      ( 0 )
+#define PLATFORM_OMNI4WHEEL    ( 1 )
+#define PLATFORM_OMNI3WHEEL    ( 2 )
+#define PLATFORM_DUALWHEEL     ( 3 )
+#define PLATFORM_ACTIVECASTER3WHEEL (4)
+#define PLATFORM_ACTIVECASTER$WHEEL (5)
+
+#define DRIVE_UNIT  ( PLATFORM_ACTIVECASTER3WHEEL )
+
+#if DRIVE_UNIT == PLATFORM_DUALWHEEL
+// 双輪キャスター関連
+#define PIN_CSB     ( 10 )    // turntableのPIN(CSB)
+#define RADIUS_R    ( 0.04 )    // wheel radius
+#define RADIUS_L    ( 0.04 )    // wheel radius
+#define W           ( 0.265 )    // tread
+#define GEARRATIO   ( 5.5 )
+#define TT_RES4     ( 4096 )    // turntableの分解能
+#define _2RES_PI    ( 2 * 2048 / M_PI ) // 駆動輪の角速度[rad/s]からRoboClawの指令値[pulses/s]に変換するための定数(2048はエンコーダ分解能) 4逓倍しているが，分母は元は2*piで，通分されている
+#define _2RES_PI_T  ( 2 * 500 / M_PI ) //  ターンテーブルの角速度[rad/s]からRoboClawの指令値[pulses/s]に変換するための定数(500はエンコーダ分解能) 4逓倍しているが，分母は元は2*piで，通分されている
+#elif DRIVE_UNIT == PLATFORM_MECHANUM
+// メカナム関連
+#define WHEEL_R		( 0.0635 )    //車輪半径
+#define TREAD_2 	( 0.296 ) //車両中心から車輪接地点までのY軸方向距離(トレッド/2)
+#define WHEELBASE_2	( 0.265 ) //車両中心から車輪接地点までのX軸方向距離(ホイールベース/2)
+#define _2RES_PI  ( 2.0 * 500 / M_PI ) //  駆動輪の角速度[rad/s]からRoboClawの指令値[pulses/s]に変換するための定数(2048はエンコーダ分解能) 4逓倍しているが，分母は元は2*piで，通分されている
+#elif DRIVE_UNIT == PLATFORM_OMNI3WHEEL
+#define WHEEL_R		( 0.019 )
+#define DIST2WHEEL  ( 0.120 )
+#define GEARRATIO   ( 51.45 )
+#define COS_PI_6    ( 0.86602540378 )
+#define SIN_PI_6    ( 0.5 )
+#define _2RES_PI    ( 2.0 * 2048 / M_PI ) // 駆動輪の角速度[rad/s]からRoboClawの指令値[pulses/s]に変換するための定数(2048はエンコーダ分解能) 4逓倍しているが，分母は元は2*piで，通分されている
+#elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
+#define WHEEL_R (0.0635)
+#define DIST2WHEEL (0.421185)               //中心からホイールまでの距離
+#define GEARRATIO   ( 1.0 )           // 車輪からエンコーダの計測軸までのギヤ比(1:1 なら 1)
+#define COS_PI_4    ( 0.70711 )         // cos(pi/4)
+#define SIN_PI_4    ( 0.70711 )         // sin(pi/4)
+#define _2RES_PI (2.0 * 1024.0 / M_PI ) //  [rad/s]からRoboClawの指令値[pulses/s]に変換するための定数(1024はエンコーダ分解能) 4逓倍しているが，分母は元は2*piで，通分されている
+#elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
+	#define OFFSET (0.04)//車輪のオフセット値
+	#define ABS_ENCRES (16384.0) //アブソリュートエンコーダの分解能
+	#define GEAR_RATIO_ENC (1.0) //操舵軸とエンコーダ計測軸とのギア比(ギアが入ってなければ1で)
+	#define GEAR_RATIO_TRANS (3.0) //モーター軸と大歯車のギア比
+	#define WHEEL_R (0.03)//車輪半径
+	#define PI (3.141592)
+	#define W1_x (0.2064)//操舵軸の座標
+	#define W1_y (0.0)
+	#define W2_x (-0.2064)
+	#define W2_y (0.2384)
+	#define W3_x (-0.2064)
+	#define W3_y (-0.2384)
+	#define MAX_RPM (400)
+	#define PIN_CSB_1 P4_0
+	#define PIN_CSB_2 P1_2
+	#define PIN_CSB_3 P2_13
+	#define PIN_PWM_1A P8_11
+	#define PIN_PWM_1B P8_13
+	#define PIN_PWM_2A P5_5
+	#define PIN_PWM_2B P5_0
+	#define PIN_PWM_3A P4_4
+	#define PIN_PWM_3B P4_5
+	#define PIN_DIR_1A P5_4
+	#define PIN_DIR_1B P5_4
+	#define PIN_DIR_2A P5_7
+	#define PIN_DIR_2B P5_6
+	#define PIN_DIR_3A P5_2
+	#define PIN_DIR_3B P5_1
+	
+	//activecaster unit1(P8_11,P8_13,P5_4,P5_3);//ピン宣言
+	//activecaster unit2(P5_5,P5_0,P5_7,P5_6);
+	//activecaster unit3(P4_4,P4_5,P5_2,P5_1);
+	/*       +x
+	          o--W1
+	          |
+	+y________|_________-y
+	          |
+	    o--W2 |     o--W3
+	         -x
+	        */
+#endif
+
+// RoboClaw関連
+#define ADR_MD1             ( 130 )
+#define ADR_MD2             ( 129 )
+
+// 自己位置推定用エンコーダ関連
+#define _2PI_RES4   ( 2 * 3.141592 / 800 ) // res = 200 分母は res*4
+#define RADIUS_X    ( 0.024 ) // X軸計測輪の半径[m]
+#define RADIUS_Y    ( 0.024 ) // Y軸計測輪の半径[m]
+// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+// >>> Controllerまわりで使用 >>>>>>>>>>>>>>>>>>>>>
+#define CON_ADACHI    ( 0 )
+#define CON_ELECOM    ( 1 )
+#define CON_DS4       ( 2 )
+
+#define CON_TYPE  ( CON_DS4 )
+
+#if CON_TYPE == CON_ADACHI
+#define MASK_BUTTON_UP    0x01
+#define MASK_BUTTON_RIGHT 0x02
+#define MASK_BUTTON_DOWN  0x04
+#define MASK_BUTTON_LEFT  0x08
+#define MASK_BUTTON_R1    0x10
+#define MASK_BUTTON_R2    0x20
+#define MASK_BUTTON_L1    0x40
+#define MASK_BUTTON_L2    0x80
+
+#define BUTTON_UP    1
+#define BUTTON_RIGHT 2
+#define BUTTON_DOWN  3
+#define BUTTON_LEFT  4
+#define BUTTON_R1    5
+#define BUTTON_R2    6
+#define BUTTON_L1    7
+#define BUTTON_L2    8
+#elif CON_TYPE == CON_ELECOM || CON_TYPE == CON_DS4
+#define MASK_BUTTON_X  0x0001
+#define MASK_BUTTON_Y  0x0002
+#define MASK_BUTTON_A  0x0004
+#define MASK_BUTTON_B  0x0008
+
+#define MASK_BUTTON_SHIKAKU 0x0001
+#define MASK_BUTTON_SANKAKU 0x0002
+#define MASK_BUTTON_BATU    0x0004
+#define MASK_BUTTON_MARU    0x0008
+
+#define MASK_BUTTON_L1     0x0010
+#define MASK_BUTTON_R1     0x0020
+#define MASK_BUTTON_L2     0x0040
+#define MASK_BUTTON_R2     0x0080
+
+#define MASK_BUTTON_PAD     0x0100 // PS4のときはPADボタン
+#define MASK_BUTTON_PS      0x0200 // PS4のときはPS
+#define MASK_BUTTON_JOY_L   0x0100
+#define MASK_BUTTON_JOY_R   0x0200
+#define MASK_BUTTON_BACK    0x0400
+#define MASK_BUTTON_START   0x0800
+#define MASK_BUTTON_SHARE   0x0400
+#define MASK_BUTTON_OPTION  0x0800
+
+#define MASK_BUTTON_UP     0x1000
+#define MASK_BUTTON_RIGHT  0x2000
+#define MASK_BUTTON_DOWN   0x4000
+#define MASK_BUTTON_LEFT   0x8000
+
+#define BUTTON_UP    13
+#define BUTTON_RIGHT 14
+#define BUTTON_DOWN  15
+#define BUTTON_LEFT  16
+#define BUTTON_R1    6
+#define BUTTON_R2    8
+#define BUTTON_L1    5
+#define BUTTON_L2    7
+#endif
+// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,164 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+//アクティブキャスタ制御用プログラム
+
+//#include "AutoControl.h"
+
+#include "LpmsMe1Peach.h"
+#include "SDclass.h"
+#include "phaseCounterPeach.h"
+#include "AMT22.h"
+#include "Filter.h"
+#include "mbed.h"
+#include "Controller.h"
+#include "define.h"
+#include "ManualControl.h"
+#include "DDSScontrol.h"
+#include "Platform.h"
+Serial pc(USBTX,USBRX,115200);
+
+ManualControl manualCon;
+Controller con(P7_4,P7_5, 115200); //TXpin, RXpin, baudrateを設定
+
+Platform _platform(1,1,1,1);
+PhaseCounter enc1(1);
+PhaseCounter enc2(2);
+int encounter1, encounter2;
+
+Serial serialLpms(P2_5, P2_6);
+LpmsMe1 lpms(&serialLpms);
+
+mySDclass mySD;
+//AutoControl autonomous;
+Ticker timer;
+DigitalIn sw_user(P6_0);
+
+DigitalOut led_blue(LED_BLUE);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_red(LED_RED);
+
+DigitalOut pin_led_1(P3_9);
+DigitalOut pin_led_2(P2_0);
+
+DigitalOut led(LED_USER);
+DigitalOut enable(P2_9);
+
+float angle = 0.0;
+bool flag10ms = false;
+
+
+void warikomi()
+{
+    //
+    static int counter1ms = 0;
+    counter1ms++;
+
+    static float count = 0;
+
+    encounter1 = enc1.getCount();
+    encounter2 = enc2.getCount();
+
+    if (counter1ms == 10) {
+        flag10ms = true;
+        counter1ms = 0;
+    }
+}
+
+int main()
+{
+    wait(1.0);
+    coords initposi = {0.0,0.0,0.0};
+    _platform.platformInit(initposi);
+    enc1.init();
+    enc2.init();
+    lpms.init();
+    //con.init(1000, 0.01*10);
+    manualCon.init();
+    
+    pc.printf("Hello World\r\n");
+    while(sw_user.read()) {
+        static int led_count=0;
+        led_count++;
+        if(led_count<10) {
+            led_blue = 1;
+        } else if(led_count >= 10 && led_count<20) {
+            led_blue = 0;
+        } else if(led_count>=20) {
+            led_count = 0;
+        }
+        wait(0.1);
+        pc.printf(">>push the button<<");
+        pc.printf("\033[1;1f");
+    }
+    pc.printf("program start");
+    wait(1.0);
+
+    led_blue = 0;
+    timer.attach(&warikomi, 0.001);
+    int count = 0;
+    int absencount = 0;
+    while (true) {
+        if (flag10ms) {
+            count++;
+            coords local_refV;
+            coords global_refV;
+            angle = lpms.get_z_angle();
+
+            uint8_t joyRx ;
+            uint8_t joyRy;
+            uint8_t joyLx;
+            uint8_t joyLy;
+            unsigned int buttonState;
+            static int count_com_error = 0;
+            bool flag_connect;
+            //con.update(); //main関数のflag内で呼び出す．
+            if(con.update()) {
+                buttonState = con.getButtonState();
+                joyRx = con.readJoyRXbyte();
+                joyRy = con.readJoyRYbyte();
+                joyLx = con.readJoyLXbyte();
+                joyLy = con.readJoyLYbyte();
+
+                led=!led;
+                led_red = 0;
+                led_green = 1;
+                count_com_error = 0;
+                enable = 1;
+                flag_connect = true;
+            } else {
+                led=0;
+                count_com_error++;
+                if(count_com_error > 50) {
+                    enable  = 0;
+                    led_green = 0;
+                    led_red = 1;
+                    //pc.printf("disconnected!!\r\n");
+                    local_refV.x = 0;
+                    local_refV.y = 0;
+                    local_refV.z = 0;
+                    flag_connect = false;
+                }
+            }
+
+////////////////////////////指令値生成///////////////////////////////////////////
+            local_refV = manualCon.getRefVel(joyLx,joyLy,joyRy);
+            global_refV.x = local_refV.x * cos(angle) + local_refV.y * sin(angle);
+            global_refV.y = local_refV.y * cos(angle) - local_refV.x * sin(angle);
+            global_refV.z = local_refV.z;
+
+            _platform.VelocityControl(global_refV);
+            
+            if (count >= 50) {
+                pin_led_1 = !pin_led_1;
+                pin_led_2 = !pin_led_2;
+                count = 0;
+            }
+            flag10ms = false;
+        }
+        thread_sleep_for(1);
+    }
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#0be7685a27f28c02737f42055f4a9c182a7b483c
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/myClasses.lib	Wed Jan 26 11:22:14 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/e5119053f6/code/ActiveCaster/#5a65fc20f8c2