Revision 0:d03e6d527de4, committed 2022-01-24

Comitter:

e5119053f6

Date:

Mon Jan 24 03:14:45 2022 +0000

Commit message:

acitivecaster;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Mon Jan 24 03:14:45 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	Mon Jan 24 03:14:45 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	Mon Jan 24 03:14:45 2022 +0000
@@ -0,0 +1,240 @@
+#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 PIN_CSB_1 P4_0
+#define PIN_CSB_2 P1_2
+#define PIN_CSB_3 P2_13
+
+// 制御周期
+#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.225)//操舵軸の座標
+	#define W1_y (0.0)
+	#define W2_x (-0.225)
+	#define W2_y (0.225)
+	#define W3_x (-0.225)
+	#define W3_y (-0.225)
+	/*       +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	Mon Jan 24 03:14:45 2022 +0000
@@ -0,0 +1,325 @@
+/* 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"
+Serial pc(USBTX,USBRX,115200);
+
+ManualControl manualCon;
+//Timer t;
+//QEI onboard_enc(P5_2, P5_3, NC, 24, &t, QEI::X4_ENCODING);
+Controller con(P7_4,P7_5, 115200); //TXpin, RXpin, baudrateを設定
+//#include "platform/mbed_thread.h"
+
+
+#define ABS_ENC_RES 16384
+#define enc_gear_ratio 3
+#define MAX_RPM 400.0
+
+double abs_enc_rad(int abscount)
+{
+    return (double)abscount * 6.28 / (ABS_ENC_RES * GEAR_RATIO_ENC);
+}
+
+PhaseCounter enc1(1);
+PhaseCounter enc2(2);
+int encounter1, encounter2;
+
+Serial serialLpms(P2_5, P2_6);
+LpmsMe1 lpms(&serialLpms);
+
+SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
+AMT203V absenc_1(&spi, PIN_CSB_1);
+AMT203V absenc_2(&spi, PIN_CSB_2);
+AMT203V absenc_3(&spi, PIN_CSB_3);
+
+mySDclass mySD;
+//AutoControl autonomous;
+
+double Vr_omega = 0;
+
+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);
+
+/*DigitalIn pin_sw_white(P5_0);
+DigitalIn pin_sw_yellow(P5_1);
+DigitalIn pin_sw_up(P7_15);
+DigitalIn pin_sw_right(P2_10);
+DigitalIn pin_sw_left(P8_1);
+DigitalIn pin_sw_down(P2_9);*/
+
+DigitalOut led(LED_USER);
+
+//DigitalOut enable_1(P3_15);
+//DigitalOut dir_1(P3_11);
+//PwmOut vel_1(P3_10);
+
+PwmOut pwm_2_a(P5_5);
+PwmOut pwm_2_b(P5_0);
+PwmOut pwm_1_a(P8_11);
+PwmOut pwm_1_b(P8_13);
+PwmOut pwm_3_a(P4_4);
+PwmOut pwm_3_b(P4_5);
+
+DigitalOut dir_2_a(P5_7);
+DigitalOut dir_2_b(P5_6);
+DigitalOut dir_1_a(P5_4);
+DigitalOut dir_1_b(P5_3);
+DigitalOut dir_3_a(P5_2);
+DigitalOut dir_3_b(P5_1);
+
+DigitalOut enable(P2_9);
+
+//DigitalOut enable_2(P3_14);
+//DigitalOut dir_2(P3_9);
+//PwmOut vel_2(P3_8);
+
+float angle = 0.0;
+bool flag10ms = false;
+
+
+void activecaster_control(int wheel_num,double vel_x,double vel_y,double vel_z,double angle_stear)
+{
+    double Wp_x,Wp_y;//固定点の座標
+
+    switch(wheel_num) {
+        case 1:
+            Wp_x = W1_x;
+            Wp_y = W1_y;
+            break;
+        case 2:
+            Wp_x = W2_x;
+            Wp_y = W2_y;
+            break;
+        case 3:
+            Wp_x = W3_x;
+            Wp_y = W3_y;
+            break;
+    }
+    double x_w1 = vel_x - Wp_x * vel_z;//固定点の速度
+    double y_w1 = vel_y + Wp_y * vel_z;
+
+    double omega_wheel = (x_w1 * cos(angle_stear) + y_w1 * sin(angle_stear)) / 0.03;//操舵角速度
+    double omega_stear = (-x_w1 * sin(angle_stear) + y_w1 * cos(angle_stear)) / 0.04;//走行角速度
+
+    double omega_a = GEAR_RATIO_TRANS * omega_wheel - GEAR_RATIO_TRANS * omega_stear;//モーターAの角速度
+    double omega_b = GEAR_RATIO_TRANS * omega_wheel + GEAR_RATIO_TRANS * omega_stear;//モーターBの角速度
+
+    double rpm_a = omega_a * 9.549;//角速度から回転数(rpm)に換算するところ
+    double rpm_b = omega_b * 9.549;
+
+    double MD_val_A = fabs(rpm_a) * 0.8 / MAX_RPM+0.1;//ESCONの指令値に変換する処理
+    double MD_val_B = fabs(rpm_b) * 0.8 / MAX_RPM+0.1;
+    if(MD_val_A>0.9) {//ESCONのPWM指令は10%から90%の間で有効なのでその範囲に収める処理
+        MD_val_A = 0.9;
+    }
+    if(MD_val_A<0.1) {
+        MD_val_A=0.1;
+    }
+
+    if(MD_val_B>0.9) {
+        MD_val_B = 0.9;
+    }
+    if(MD_val_B<0.1) {
+        MD_val_B=0.1;
+    }
+///////////////////////回転方向の指定/////////////////////////////////////////////
+    switch(wheel_num) {
+        case 1:
+            if(rpm_a>0) {
+                dir_1_a = 1;
+            } else {
+                dir_1_a = 0;
+            }
+            if(rpm_b>0) {
+                dir_1_b = 0;
+            } else {
+                dir_1_b = 1;
+            }
+            pwm_1_a.write(MD_val_A);
+            pwm_1_b.write(MD_val_B);
+            break;
+        case 2:
+
+            if(rpm_a>0) {
+                dir_2_a = 1;
+            } else {
+                dir_2_a = 0;
+            }
+            if(rpm_b>0) {
+                dir_2_b = 0;
+            } else {
+                dir_2_b = 1;
+            }
+
+            pwm_2_a.write(MD_val_A);
+            pwm_2_b.write(MD_val_B);
+            break;
+        case 3:
+            if(rpm_a>0) {
+                dir_3_a = 1;
+            } else {
+                dir_3_a = 0;
+            }
+            if(rpm_b>0) {
+                dir_3_b = 0;
+            } else {
+                dir_3_b = 1;
+            }
+            pwm_3_a.write(MD_val_A);
+            pwm_3_b.write(MD_val_B);
+
+            break;
+    }
+    //pc.printf("velx:%lf vely:%lf vel_z:%lf omega_a %lf omega_b %lf dir_a:%d dir_b:%d angle:%lf\r\n",vel_x,vel_y,vel_z,omega_a,omega_b,dir_3_a.read(),dir_3_b.read(),angle_stear);
+    //pc.printf("omega_a %lf omega_b %lf dir_a:%d dir_b:%d angle:%lf\r\n",omega_a,omega_b,dir_2_a.read(),dir_2_b.read(),angle_stear);
+    //pc.printf("rpm_a:%lf rpm_b:%lf\r\n",rpm_a,rpm_b);
+    
+}
+
+
+void warikomi()
+{
+    //
+    static int counter1ms = 0;
+    counter1ms++;
+
+    static float count = 0;
+
+    encounter1 = enc1.getCount();
+    encounter2 = enc2.getCount();
+
+    if (counter1ms == 10) {
+        flag10ms = true;
+        counter1ms = 0;
+        // RGB LED を良い感じに光らせるための処理
+
+
+    }
+    // pc.printf("test");
+}
+
+int main()
+{
+
+    //pc.baud(115200);
+    pc.printf("program start");
+    wait(1.0);
+
+    enc1.init();
+    enc2.init();
+    lpms.init();
+    //con.init(1000, 0.01*10);
+    manualCon.init();
+    pc.printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
+    pc.printf("Hello World\r\n");
+    while(sw_user.read()) {
+        static int led_count=0;
+        led_count ++;
+        if(led_count<1000) {
+            led_blue = 1;
+        }
+        if(led_count >= 1000 && led_count<2000) {
+            led_blue = 0;
+        }
+        if(led_count>=2000) {
+            led_count = 0;
+        }
+        wait(0.5);
+    }
+    wait(1.0);
+    led_blue = 0;
+    timer.attach(&warikomi, 0.001);
+    int count = 0;
+    int absencount = 0;
+    while (true) {
+        if (flag10ms) {
+            count++;
+            coords 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;
+            //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;
+            } else {
+                led=0;
+                count_com_error++;
+                if(count_com_error > 50) {
+                    enable  = 0;
+                    led_green = 0;
+                    led_red = 1;
+                    pc.printf("disconnected!!\r\n");
+                    refV.x = 0;
+                    refV.y = 0;
+                    refV.z = 0;
+                }
+            }
+            if((buttonState & MASK_BUTTON_PAD) == MASK_BUTTON_PAD) {
+                absenc_1.setZeroPos();
+                absenc_2.setZeroPos();
+                absenc_3.setZeroPos();
+            }
+////////////////////////////指令値生成///////////////////////////////////////////
+            refV = manualCon.getRefVel(joyLx,joyLy,joyRy);
+            //activecaster_control(1,refV.x,refV.y,refV.z,abs_enc_rad(absenc_1.getEncount()));//車輪1の制御
+            int encount_2 = absenc_2.getEncount();
+            int encount_3 = absenc_3.getEncount();
+            double enc_rad_2 = (double)encount_2 / ABS_ENCRES * PI * -2;
+            double enc_rad_3 = (double)encount_3 / ABS_ENCRES * PI * -2;
+
+            activecaster_control(2,refV.x,refV.y,refV.z,enc_rad_2);//車輪2の制御
+            activecaster_control(3,refV.x,refV.y,refV.z,enc_rad_3);//車輪3の制御
+
+            //pc.printf("count:%d enc_Rad_2:%.2lf",encount_2,enc_rad_2);
+            pc.printf("x:%.2lf y:%.2lf z:%.2lf enc_2:%lf\r\n",refV.x,refV.y,refV.z,enc_rad_2);
+
+            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	Mon Jan 24 03:14:45 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	Mon Jan 24 03:14:45 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/e5119053f6/code/ActiveCaster/#5e4f1e288e2a