Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN

Dependencies:   mbed TrackedVehicle PS3

Files at this revision

API Documentation at this revision

Comitter:
st17099ng
Date:
Fri Nov 26 00:35:42 2021 +0000
Child:
1:ca42cf6ae77e
Commit message:
First commit hairobo2021_NUCLEO-F446RE

Changed in this revision

PS3.lib Show annotated file Show diff for this revision Revisions of this file
README.h Show annotated file Show diff for this revision Revisions of this file
Robot_Definition.h Show annotated file Show diff for this revision Revisions of this file
TrackedVehicle.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3.lib	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/st17099ng/code/PS3/#0fdf068ce9f4
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/README.h	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,197 @@
+/**************************************************************************************************
+
+program変更履歴
+--------------------------------------------------------------------
+ 2021/10/11 R.I.P.
+ TrackedVehicleにConvert_PS3toMsgとして移行
+ PS3Data -> Duty変換関数
+void Data_To_Duty(void)
+{
+    bool r1 = ps3.getButtonState( R1 );
+    bool l1 = ps3.getButtonState( L1 );
+    bool r2 = ps3.getButtonState( R2 );
+    bool l2 = ps3.getButtonState( L2 );
+    MD1_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F;
+    MD2_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F;
+    MD3_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F;
+    MD4_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F;
+
+              モジュール1
+      信地右旋回
+    if( ( r1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) {
+
+        MD1_Duty = BREAK_DUTY;
+        MD2_Duty = HALF_CCW_DUTY;
+
+          信地左旋回
+    } else if( ( l1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) {
+
+        MD1_Duty = HALF_CCW_DUTY;
+        MD2_Duty = BREAK_DUTY;
+
+          超信地右旋回
+    } else if( r1 == 1) {
+
+        MD1_Duty = HALF_CW_DUTY;
+        MD2_Duty = HALF_CCW_DUTY;
+
+          超信地左旋回
+    } else if( l1 == 1 ) {
+
+        MD1_Duty = HALF_CCW_DUTY;
+        MD2_Duty = HALF_CW_DUTY;
+
+          前進(JoyStickの閾値32の時
+          計算式:DEC(32*2) + HEX(0x7F) = 0xBF
+    } else if( MD1_MaxDuty >= 0xBF ) {
+
+//        MD1_Duty = CCW10_DUTY;
+        MD1_Duty = HALF_CCW_DUTY;
+        MD2_Duty = HALF_CCW_DUTY;
+
+         後進(JoyStickの閾値-32の時
+          計算式:DEC(-32*2) + HEX(0x7F) = 0x3F
+    } else if( MD1_MaxDuty <= 0x3F ) {
+
+        MD1_Duty = CW10_DUTY;
+        MD1_Duty = HALF_CW_DUTY;
+        MD2_Duty = HALF_CW_DUTY;
+
+          停止
+    } else {
+
+        MD1_Duty = BREAK_DUTY;
+        MD2_Duty = BREAK_DUTY;
+
+    }
+
+
+              モジュール2
+      信地右旋回
+    if( ( r2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) {
+
+        MD3_Duty = BREAK_DUTY;
+        MD4_Duty = HALF_CCW_DUTY;
+
+          信地左旋回
+    } else if( ( l2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) {
+
+        MD3_Duty = HALF_CCW_DUTY;
+        MD4_Duty = BREAK_DUTY;
+
+          超信地右旋回
+    } else if( r2 == 1) {
+
+        MD3_Duty = HALF_CW_DUTY;
+        MD4_Duty = HALF_CCW_DUTY;
+
+          超信地左旋回
+    } else if( l2 == 1 ) {
+
+        MD3_Duty = HALF_CCW_DUTY;
+        MD4_Duty = HALF_CW_DUTY;
+
+          前進(JoyStickの閾値32の時
+          計算式:DEC(32*2) + HEX(0x7F) = 0xBF
+    } else if( MD3_MaxDuty >= 0xBF ) {
+
+        MD3_Duty = HALF_CCW_DUTY;
+        MD4_Duty = HALF_CCW_DUTY;
+
+         後進(JoyStickの閾値-32の時
+          計算式:DEC(-32*2) + HEX(0x7F) = 0x3F
+    } else if( MD3_MaxDuty <= 0x3F ) {
+
+        MD3_Duty = HALF_CW_DUTY;
+        MD4_Duty = HALF_CW_DUTY;
+
+          停止
+    } else {
+
+        MD3_Duty = BREAK_DUTY;
+        MD4_Duty = BREAK_DUTY;
+
+    }
+}
+---台形制御をMDのプログラムに導入したため---
+---台形制御関数---
+void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty)
+{
+    int Diff_Duty = MD_Duty - msg->data[2];
+    if ( msg->data[2] == MD_Duty ) {
+
+        msg->data[2] = MD_Duty;
+
+    } else if( Diff_Duty >= 3 ) {
+
+        msg->data[2] += ADJUST_DUTY;
+
+    } else if( ( Diff_Duty < 3 ) && ( Diff_Duty > 0 ) ) {
+
+        msg->data[2] += Diff_Duty;
+
+    } else if( ( Diff_Duty > -3 ) && ( Diff_Duty < 0 ) ) {
+
+        msg->data[2] += Diff_Duty;
+
+    } else if( Diff_Duty <= 3 ) {
+
+        msg->data[2] -= ADJUST_DUTY;
+
+    }
+}
+ 2021/08/26 R.I.P.
+--void Soft_Duty(void) -> void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty) に変更---
+void Soft_Duty(void)
+{
+    if(msg1.data[2] == MD1_Duty) {
+        msg1.data[2] = MD1_Duty;
+    } else if(msg1.data[2] <= MD1_Duty) {
+        msg1.data[2] += ADJUST_DUTY;
+    } else if(msg1.data[2] >= MD1_Duty) {
+        msg1.data[2] -= ADJUST_DUTY;
+    }
+
+    if(msg2.data[2] == MD2_Duty) {
+        msg2.data[2] = MD2_Duty;
+    } else if(msg2.data[2] <= MD2_Duty) {
+        msg2.data[2] += ADJUST_DUTY;
+    } else if(msg2.data[2] >= MD2_Duty) {
+        msg2.data[2] -= ADJUST_DUTY;
+    }
+}
+
+
+---[実装未定]PS3コン切替式非常停止---
+    if( Old_Emg_Flag != Emg_Flag ) {
+        if( ( Old_Emg_Flag == 0 ) && ( Emg_Flag == 1 ) ) {//立ち上がり
+            led1 = 0;
+            emg_stop_tri = 0;//停止
+        } else if( ( Old_Emg_Flag == 1 ) && ( Emg_Flag == 0 ) ) {//立下り
+            led1 = 0;
+            emg_stop_tri = 0;//停止
+        }
+    } else {
+        Old_Emg_Flag = Emg_Flag;
+    }
+
+
+---ノードが増えると割込みによる運用が出来なくなる---
+    ticker.attach(&CAN1_Trans, 0.05);
+---CAN1送信関数---
+void CAN1_Trans(void)
+{
+    if( !(can1.write(msg1)) ) {
+        pc.printf("\033[1;1Hmsg1 failed");
+    }
+    if( !(can1.write(msg2)) ) {
+        pc.printf("\033[2;1Hmsg2 failed");
+    }
+    if( !(can1.write(msg3)) ) {
+        pc.printf("\033[3;1Hmsg3 failed");
+    }
+    if( !(can1.write(msg4)) ) {
+        pc.printf("\033[4;1Hmsg4 failed");
+    }
+}
+***************************************************************************************************/
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot_Definition.h	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,50 @@
+#ifndef ROBOT_DEFINITION_H
+#define ROBOT_DEFINITION_H
+
+enum sequence_number {
+    STOP_STATUS,
+    INITIAL_STATUS,
+    MANUAL_STATUS,
+    ZERO_POINT_TURN,
+    OPPOSITE_PHASE_RIGHT_TURN,
+    RIGHT_SOFT_TURN,
+    RIGHT_SOFT_TURN_BACK,
+    OPPOSITE_PHASE_LEFT_TURN,
+    LEFT_SOFT_TURN,
+    LEFT_SOFT_TURN_BACK,
+    RIGHT90_TURN,
+    LEFT90_TURN,
+    MODULE_FRONT,
+    MODULE_BACK,
+    ROBOT_CW_TURN,
+    ROBOT_CCW_TURN
+};
+
+enum ControlNumber {
+    PS3MODE,
+    GUIMODE
+};
+
+enum Duty {
+    CW95_DUTY       = 0xF8,
+    CW80_DUTY       = 0xE4,
+    CW70_DUTY       = 0xD7,
+    CW60_DUTY       = 0xCA,
+    CW50_DUTY       = 0xBE,
+    CW40_DUTY       = 0xB3,
+    CW30_DUTY       = 0xA6,
+    CW20_DUTY       = 0x99,
+    CW10_DUTY       = 0x8C,
+    BREAK_DUTY      = 0x7F,
+    CCW10_DUTY      = 0x72,
+    CCW20_DUTY      = 0x65,
+    CCW30_DUTY      = 0x58,
+    CCW40_DUTY      = 0x4B,
+    CCW50_DUTY      = 0x3F,
+    CCW60_DUTY      = 0x32,
+    CCW70_DUTY      = 0x26,
+    CCW80_DUTY      = 0x19,
+    CCW95_DUTY      = 0x07,
+    ADJUST_DUTY     = 0x03
+};
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TrackedVehicle.lib	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/st17099ng/code/TrackedVehicle/#b5561184f201
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,617 @@
+#include "mbed.h"
+#include "TrackedVehicle.h"
+#include "Robot_Definition.h"
+#include "PS3.h"
+
+void Update_Robot_Status(void);
+void Lift_Control(void);
+void Paint_Control(void);
+void Cable_Control(void);
+void Outrigger_Control(void);
+void GUI_to_Status(void);
+void CAN1_Trans(CANMessage *msg);
+void CAN1_Rcv(void);
+void GUI_Receive(void);
+void GUI_Safety_Stop(void);
+void Buzzer_Toggle(void);
+void Emg_Stop(void);
+
+
+char GUIData = 0x00;
+char ResetData[8]   = {0, 0, BREAK_DUTY, 0,0,0,0,0};
+char MD5Data[8]   = {0, 5, BREAK_DUTY, 0,0,0,0,0};
+char MD6Data[8]   = {0, 6, BREAK_DUTY, 0,0,0,0,0};
+char MD7Data[8]   = {0, 7, BREAK_DUTY, 0,0,0,0,0};
+char MD8Data[8]   = {0, 8, BREAK_DUTY, 0,0,0,0,0};
+char MD9Data[8]   = {0, 9, BREAK_DUTY, 0,0,0,0,0};
+char MD10Data[8]   = {0, 10, BREAK_DUTY, 0,0,0,0,0};
+int8_t RightJoyX    = 0;
+int8_t RightJoyY    = 0;
+int8_t LeftJoyX     = 0;
+int8_t LeftJoyY     = 0;
+uint16_t Module1Encoder = 4096;
+uint16_t Module2Encoder = 4096;
+uint16_t LiftEncoder = 2400;
+short ControlMode       = GUIMODE;
+short RobotStatus       = INITIAL_STATUS;
+short Old_RobotStatus   = INITIAL_STATUS;
+short Module1Status     = INITIAL_STATUS;
+short Module2Status     = INITIAL_STATUS;
+
+bool MaintainAngle               = false;//sita
+bool ZeroPointTurn               = false;//ue
+bool RightTurn                   = false;//migi
+bool LeftTurn                    = false;//hidari
+bool CWTurn                      = false;//R1
+bool CCWTurn                     = false;//L1
+bool OppositePhase_RightTurn     = false;//maru
+bool OppositePhase_LeftTurn      = false;//sikaku
+bool Right2                      = false;//R2
+bool Left2                       = false;//L2
+bool LiftUp                      = false;//sankaku
+bool LiftDown                    = false;//batsu
+bool Old_MaintainAngle           = false;
+bool Old_ZeroPointTurn           = false;
+bool Old_RightTurn               = false;
+bool Old_LeftTurn                = false;
+bool Old_CWTurn                  = false;
+bool Old_CCWTurn                 = false;
+bool Old_OppositePhase_RightTurn = false;
+bool Old_OppositePhase_LeftTurn  = false;
+bool Old_LiftUp                  = false;
+bool Old_LiftDown                = false;
+
+bool EmgFlag = true;
+bool Start = false;
+bool Select = false;
+bool Old_Start = false;
+bool Old_Select = false;
+bool Old_emg_stop_sw = false;
+
+Ticker GUI_Safety_Timer;
+Ticker EMG_Buzzer_Timer;
+CAN can1(PA_11, PA_12,500000);
+Serial pc(USBTX,USBRX,115200);
+Serial RaspPi(PC_12, PD_2,115200);
+
+PS3 ps3(PC_10,PC_11);
+TrackedVehicle Module1(0x7E0, 0x7E1);
+TrackedVehicle Module2(0x7E2, 0x7E3);
+
+DigitalOut led1(LED1);
+DigitalOut emg_stop_tri(PB_14);
+DigitalOut Ps3_Led(PC_0);
+DigitalOut Pwr_Led(PC_1);
+DigitalOut Emg_Led(PB_0);
+DigitalOut Gui_Led(PA_4);
+DigitalOut buzzer(PA_1);
+DigitalIn  emg_stop_sw(PB_13);
+
+CANMessage Rcv_Msg;
+CANMessage MD1_Msg(0x7E0,ResetData,3,CANData,CANStandard);//Module1Right
+CANMessage MD2_Msg(0x7E1,ResetData,3,CANData,CANStandard);//Module1Left
+CANMessage MD3_Msg(0x7E2,ResetData,3,CANData,CANStandard);//Module2Right
+CANMessage MD4_Msg(0x7E3,ResetData,3,CANData,CANStandard);//Module2Left
+CANMessage MD5_Msg(0x7E4,MD5Data,3,CANData,CANStandard);//lift
+CANMessage MD6_Msg(0x7E5,MD6Data,3,CANData,CANStandard);//X
+CANMessage MD7_Msg(0x7E6,MD7Data,3,CANData,CANStandard);//Y
+CANMessage MD8_Msg(0x7E7,MD8Data,3,CANData,CANStandard);//Z
+CANMessage MD9_Msg(0x7E8,MD9Data,3,CANData,CANStandard);//Cable
+CANMessage MD10_Msg(0x7E9,MD10Data,3,CANData,CANStandard);//Outrigger
+
+int main()
+{
+    emg_stop_tri = 0;
+    Ps3_Led = 0;
+    Pwr_Led = 1;
+    Emg_Led = 1;
+    Gui_Led = 1;
+    emg_stop_sw.mode( PullUp );
+//    コメント外すとうるさいよ
+//    EMG_Buzzer_Timer.attach(&Buzzer_Toggle, 1.5);
+    can1.attach(&CAN1_Rcv,CAN::RxIrq);
+//    pc.attach(&GUI_Receive,Serial::RxIrq);
+    RaspPi.attach(&GUI_Receive,Serial::RxIrq);
+    CAN1_Trans(&MD1_Msg);
+    CAN1_Trans(&MD2_Msg);
+    CAN1_Trans(&MD3_Msg);
+    CAN1_Trans(&MD4_Msg);
+    CAN1_Trans(&MD5_Msg);
+    CAN1_Trans(&MD6_Msg);
+    CAN1_Trans(&MD7_Msg);
+    CAN1_Trans(&MD8_Msg);
+    CAN1_Trans(&MD9_Msg);
+    CAN1_Trans(&MD10_Msg);
+    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET);
+    while(1) {
+        Emg_Stop();
+        if(ControlMode == PS3MODE) {
+            Update_Robot_Status();
+        } else if(ControlMode == GUIMODE) {
+            GUI_to_Status();
+        }
+        Lift_Control();
+        Paint_Control();
+        Cable_Control();
+        Outrigger_Control();
+        switch (RobotStatus) {
+            case STOP_STATUS:
+                Module1.Stop_Robot();
+                Module2.Stop_Robot();
+                break;
+            //1,2(前後のみ)
+            case ZERO_POINT_TURN:
+                Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY);
+                Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY);
+                break;
+
+            //3,4,5,6(左右緩旋回前後)
+            case OPPOSITE_PHASE_RIGHT_TURN:
+            case OPPOSITE_PHASE_LEFT_TURN:
+            case RIGHT_SOFT_TURN:
+            case LEFT_SOFT_TURN:
+            case RIGHT_SOFT_TURN_BACK:
+            case LEFT_SOFT_TURN_BACK:
+                Module1.Control_Turn(Module1Status);
+                Module2.Control_Turn(Module2Status);
+                break;
+
+            //7,8(90°旋回と前後)
+            case RIGHT90_TURN:
+            case LEFT90_TURN:
+                Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY);
+                Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY);
+                break;
+
+            //9,10(全体CW,CW)
+            case ROBOT_CW_TURN:
+            case ROBOT_CCW_TURN:
+                Module1.Control_Robot_Turn(Module1Status, RobotStatus);
+                Module2.Control_Robot_Turn(Module2Status, RobotStatus);
+                break;
+
+            //ワカラン
+            case INITIAL_STATUS:
+                Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY);
+                Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY);
+                break;
+
+            //マニュアル操作
+            case MANUAL_STATUS:
+                Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY);
+                Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY);
+                break;
+        }
+        CAN1_Trans(Module1.Get_RightMsg());
+        CAN1_Trans(Module1.Get_LeftMsg());
+        CAN1_Trans(Module2.Get_RightMsg());
+        CAN1_Trans(Module2.Get_LeftMsg());
+        CAN1_Trans(&MD5_Msg);
+        CAN1_Trans(&MD6_Msg);
+        CAN1_Trans(&MD7_Msg);
+        CAN1_Trans(&MD8_Msg);
+        CAN1_Trans(&MD9_Msg);
+        CAN1_Trans(&MD10_Msg);
+    }
+}
+/* ここまでmain文 */
+
+/* PS3 -> ロボットステータス更新関数 */
+void Update_Robot_Status(void)
+{
+    MaintainAngle           = ps3.getButtonState( sita );
+    ZeroPointTurn           = ps3.getButtonState( ue );
+    RightTurn               = ps3.getButtonState( migi );
+    LeftTurn                = ps3.getButtonState( hidari );
+    CWTurn                  = ps3.getButtonState( R1 );
+    CCWTurn                 = ps3.getButtonState( L1 );
+    Right2                  = ps3.getButtonState( R2 );
+    Left2                   = ps3.getButtonState( L2 );
+    OppositePhase_RightTurn = ps3.getButtonState( maru );
+    OppositePhase_LeftTurn  = ps3.getButtonState( sikaku );
+    RightJoyY               = ps3.getRightJoystickYaxis();
+    LeftJoyY                = ps3.getLeftJoystickYaxis();
+
+    if( Start != true && Select != true) {
+        if( (ZeroPointTurn == true) && (Old_ZeroPointTurn == false) ) {
+            //正面
+            Module1.Set_TargetPulse(4096);
+            Module2.Set_TargetPulse(4096);
+            Module1Status = ZERO_POINT_TURN;
+            Module2Status = ZERO_POINT_TURN;
+            RobotStatus   = ZERO_POINT_TURN;
+        } else if( (RightTurn == true) && (Old_RightTurn == false) ) {
+            //右旋回90°
+            Module1.Set_TargetPulse(6144);
+            Module2.Set_TargetPulse(6144);
+            Module1Status = RIGHT90_TURN;
+            Module2Status = RIGHT90_TURN;
+            RobotStatus   = RIGHT90_TURN;
+//        } else if( (LeftTurn == true) && (Old_LeftTurn == false) ) {
+//            //左旋回90°
+//            Module1.Set_TargetPulse(2048);
+//            Module2.Set_TargetPulse(2048);
+//            Module1Status = LEFT90_TURN;
+//            Module2Status = LEFT90_TURN;
+//            RobotStatus   = LEFT90_TURN;
+        } else if( (OppositePhase_RightTurn == true) && (Old_OppositePhase_RightTurn == false) ) {
+            //逆位相右旋回±30°
+            Module1.Set_TargetPulse(4778);
+            Module2.Set_TargetPulse(3414);
+            Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
+            Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
+            RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
+        } else if( (OppositePhase_LeftTurn == true) && (Old_OppositePhase_LeftTurn == false) ) {
+            //逆位相左旋回±30°
+            Module1.Set_TargetPulse(3414);
+            Module2.Set_TargetPulse(4778);
+            Module1Status = OPPOSITE_PHASE_LEFT_TURN;
+            Module2Status = OPPOSITE_PHASE_LEFT_TURN;
+            RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
+        } else if( LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) {
+            //全体右緩旋回
+            Module1.Set_TargetPulse(4778);
+            Module2.Set_TargetPulse(3414);
+            Module1Status = RIGHT_SOFT_TURN;
+            Module2Status = RIGHT_SOFT_TURN;
+            RobotStatus   = RIGHT_SOFT_TURN;
+        } else if( LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) {
+            //全体後ろ右緩旋回
+            Module1.Set_TargetPulse(4778);
+            Module2.Set_TargetPulse(3414);
+            Module1Status = RIGHT_SOFT_TURN_BACK;
+            Module2Status = RIGHT_SOFT_TURN_BACK;
+            RobotStatus   = RIGHT_SOFT_TURN_BACK;
+        } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == RIGHT_SOFT_TURN || RobotStatus == RIGHT_SOFT_TURN_BACK) ) {
+            //全体右緩旋回停止->逆位相右旋回±30°
+            Module1.Set_TargetPulse(4778);
+            Module2.Set_TargetPulse(3414);
+            Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
+            Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
+            RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
+        } else if(LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) {
+            //全体左緩旋回
+            Module1.Set_TargetPulse(3414);
+            Module2.Set_TargetPulse(4778);
+            Module1Status = LEFT_SOFT_TURN;
+            Module2Status = LEFT_SOFT_TURN;
+            RobotStatus   = LEFT_SOFT_TURN;
+        } else if(LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) {
+            //全体後ろ左緩旋回
+            Module1.Set_TargetPulse(3414);
+            Module2.Set_TargetPulse(4778);
+            Module1Status = LEFT_SOFT_TURN_BACK;
+            Module2Status = LEFT_SOFT_TURN_BACK;
+            RobotStatus   = LEFT_SOFT_TURN_BACK;
+        } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == LEFT_SOFT_TURN || RobotStatus == LEFT_SOFT_TURN_BACK) ) {
+            //全体左緩旋回停止->逆位相左旋回±45°
+            Module1.Set_TargetPulse(3414);
+            Module2.Set_TargetPulse(4778);
+            Module1Status = OPPOSITE_PHASE_LEFT_TURN;
+            Module2Status = OPPOSITE_PHASE_LEFT_TURN;
+            RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
+        } else if( (CWTurn == true) && (RobotStatus == RIGHT90_TURN) ) {
+            //全体CW
+            Module1.Set_TargetPulse(6144);
+            Module2.Set_TargetPulse(6144);
+            Module1Status = MODULE_FRONT;
+            Module2Status = MODULE_BACK;
+            RobotStatus   = ROBOT_CW_TURN;
+        } else if( (CCWTurn == true) && (RobotStatus == RIGHT90_TURN) ) {
+            //全体CCW
+            Module1.Set_TargetPulse(6144);
+            Module2.Set_TargetPulse(6144);
+            Module1Status = MODULE_BACK;
+            Module2Status = MODULE_FRONT;
+            RobotStatus   = ROBOT_CCW_TURN;
+        } else if( (CWTurn != true && CCWTurn != true) && (RobotStatus == ROBOT_CW_TURN || RobotStatus == ROBOT_CCW_TURN) ) {
+            //全体左右超信地旋回 -> 右旋回90°
+            Module1.Set_TargetPulse(6144);
+            Module2.Set_TargetPulse(6144);
+            Module1Status = RIGHT90_TURN;
+            Module2Status = RIGHT90_TURN;
+            RobotStatus   = RIGHT90_TURN;
+        } else if( (MaintainAngle == true) && (Old_MaintainAngle == false) ) {
+            //十字ボタン下でその角度維持
+            Module1.Set_TargetPulse(Module1.Get_ModulePulse());
+            Module2.Set_TargetPulse(Module2.Get_ModulePulse());
+            Module1Status = INITIAL_STATUS;
+            Module2Status = INITIAL_STATUS;
+            RobotStatus   = INITIAL_STATUS;
+        }
+    } else if( (Start == true) && (Old_Start == false) ) {
+        Module1Status = MANUAL_STATUS;
+        Module2Status = MANUAL_STATUS;
+        RobotStatus   = MANUAL_STATUS;
+    }
+    Old_MaintainAngle           = MaintainAngle;
+    Old_ZeroPointTurn           = ZeroPointTurn;
+    Old_RightTurn               = RightTurn;
+    Old_LeftTurn                = LeftTurn;
+    Old_CWTurn                  = CWTurn;
+    Old_CCWTurn                 = CCWTurn;
+    Old_OppositePhase_RightTurn = OppositePhase_RightTurn;
+    Old_OppositePhase_LeftTurn  = OppositePhase_LeftTurn;
+    Old_Start                   = Start;
+}
+
+/* Liftの処理関数 */
+void Lift_Control(void)
+{
+    LiftUp   = ps3.getButtonState( sankaku );
+    LiftDown = ps3.getButtonState( batu );
+    if( LiftUp == true || GUIData == 0x0C ) {
+        //LIFT UP
+        MD5_Msg.data[2] = CW50_DUTY;
+    } else if( LiftDown == true || GUIData == 0x0D ) {
+        //LIFT DOWN
+        MD5_Msg.data[2] = CCW50_DUTY;
+    } else {
+        MD5_Msg.data[2] = BREAK_DUTY;
+    }
+}
+
+/* Paintの処理関数 */
+void Paint_Control(void)
+{
+    MD6_Msg.data[2] = BREAK_DUTY;
+    MD7_Msg.data[2] = BREAK_DUTY;
+    MD8_Msg.data[2] = BREAK_DUTY;
+    if(GUIData == 0x0F) {
+        MD6_Msg.data[2] = CW50_DUTY;
+    } else if(GUIData == 0x10) {
+        MD6_Msg.data[2] = CCW50_DUTY;
+    } else if(GUIData == 0x11) {
+        MD7_Msg.data[2] = CW50_DUTY;
+    } else if(GUIData == 0x12) {
+        MD7_Msg.data[2] = CCW50_DUTY;
+    } else if(GUIData == 0x13) {
+        MD8_Msg.data[2] = CW50_DUTY;
+    } else if(GUIData == 0x14) {
+        MD8_Msg.data[2] = CCW50_DUTY;
+    } else {
+        MD6_Msg.data[2] = BREAK_DUTY;
+        MD7_Msg.data[2] = BREAK_DUTY;
+        MD8_Msg.data[2] = BREAK_DUTY;
+    }
+}
+/* Cableの処理関数 */
+void Cable_Control(void)
+{
+    if(GUIData == 0x16) {
+        //Cable収納
+        MD9_Msg.data[2] = CW50_DUTY;
+    } else if(GUIData == 0x17) {
+        //Cable排出
+        MD9_Msg.data[2] = CCW50_DUTY;
+    } else {
+        MD9_Msg.data[2] = BREAK_DUTY;
+    }
+}
+
+/* Outriggerの処理関数 */
+void Outrigger_Control(void)
+{
+    if(GUIData == 0x19) {
+        //Outrigger展開
+        MD10_Msg.data[2] = CW50_DUTY;
+    } else if(GUIData == 0x1A) {
+        //Outrigger収納
+        MD10_Msg.data[2] = CCW50_DUTY;
+    } else {
+        MD10_Msg.data[2] = BREAK_DUTY;
+    }
+}
+
+/* GUI -> Status変換関数 */
+void GUI_to_Status(void)
+{
+    LeftJoyY = 0;
+    if( GUIData == 0x01 ) {
+        //前進
+        Module1.Set_TargetPulse(4096);
+        Module2.Set_TargetPulse(4096);
+        Module1Status = ZERO_POINT_TURN;
+        Module2Status = ZERO_POINT_TURN;
+        RobotStatus   = ZERO_POINT_TURN;
+        LeftJoyY = 63;
+    } else if( GUIData == 0x02 ) {
+        //後進
+        Module1.Set_TargetPulse(4096);
+        Module2.Set_TargetPulse(4096);
+        Module1Status = ZERO_POINT_TURN;
+        Module2Status = ZERO_POINT_TURN;
+        RobotStatus   = ZERO_POINT_TURN;
+        LeftJoyY = -63;
+    } else if( GUIData == 0x03 ) {
+        //右緩旋回
+        Module1.Set_TargetPulse(4778);
+        Module2.Set_TargetPulse(3414);
+        Module1Status = RIGHT_SOFT_TURN;
+        Module2Status = RIGHT_SOFT_TURN;
+        RobotStatus   = RIGHT_SOFT_TURN;
+    } else if( GUIData == 0x04 ) {
+        //右緩後旋回
+        Module1.Set_TargetPulse(4778);
+        Module2.Set_TargetPulse(3414);
+        Module1Status = RIGHT_SOFT_TURN_BACK;
+        Module2Status = RIGHT_SOFT_TURN_BACK;
+        RobotStatus   = RIGHT_SOFT_TURN_BACK;
+    } else if( GUIData == 0x05 ) {
+        //左緩旋回
+        Module1.Set_TargetPulse(3414);
+        Module2.Set_TargetPulse(4778);
+        Module1Status = LEFT_SOFT_TURN;
+        Module2Status = LEFT_SOFT_TURN;
+        RobotStatus   = LEFT_SOFT_TURN;
+    } else if( GUIData == 0x06 ) {
+        //左緩後旋回
+        Module1.Set_TargetPulse(3414);
+        Module2.Set_TargetPulse(4778);
+        Module1Status = LEFT_SOFT_TURN_BACK;
+        Module2Status = LEFT_SOFT_TURN_BACK;
+        RobotStatus   = LEFT_SOFT_TURN_BACK;
+    } else if( GUIData == 0x07 ) {
+        //右向き前進
+        Module1.Set_TargetPulse(6144);
+        Module2.Set_TargetPulse(6144);
+        Module1Status = RIGHT90_TURN;
+        Module2Status = RIGHT90_TURN;
+        RobotStatus   = RIGHT90_TURN;
+        LeftJoyY = 63;
+    } else if( GUIData == 0x08 ) {
+        //右向き後進
+        Module1.Set_TargetPulse(6144);
+        Module2.Set_TargetPulse(6144);
+        Module1Status = RIGHT90_TURN;
+        Module2Status = RIGHT90_TURN;
+        RobotStatus   = RIGHT90_TURN;
+        LeftJoyY = -63;
+    } else if( GUIData == 0x09 ) {
+        //全体CW
+        Module1.Set_TargetPulse(6144);
+        Module2.Set_TargetPulse(6144);
+        Module1Status = MODULE_FRONT;
+        Module2Status = MODULE_BACK;
+        RobotStatus   = ROBOT_CW_TURN;
+    } else if( GUIData == 0x0A ) {
+        //全体CCW
+        Module1.Set_TargetPulse(6144);
+        Module2.Set_TargetPulse(6144);
+        Module1Status = MODULE_BACK;
+        Module2Status = MODULE_FRONT;
+        RobotStatus   = ROBOT_CCW_TURN;
+    } else if( GUIData == 0xFF ) {
+        //0°(正面)
+        Module1.Set_TargetPulse(4096);
+        Module2.Set_TargetPulse(4096);
+        Module1Status = ZERO_POINT_TURN;
+        Module2Status = ZERO_POINT_TURN;
+        RobotStatus   = ZERO_POINT_TURN;
+    } else if( GUIData == 0xFE ) {
+        //±30°(右緩旋回)
+        Module1.Set_TargetPulse(4778);
+        Module2.Set_TargetPulse(3414);
+        Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
+        Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
+        RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
+    } else if( GUIData == 0xFD ) {
+        //±30°(左緩旋回)
+        Module1.Set_TargetPulse(3414);
+        Module2.Set_TargetPulse(4778);
+        Module1Status = OPPOSITE_PHASE_LEFT_TURN;
+        Module2Status = OPPOSITE_PHASE_LEFT_TURN;
+        RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
+    } else if( GUIData == 0xFC ) {
+        //90°(右向き)
+        Module1.Set_TargetPulse(6144);
+        Module2.Set_TargetPulse(6144);
+        Module1Status = RIGHT90_TURN;
+        Module2Status = RIGHT90_TURN;
+        RobotStatus   = RIGHT90_TURN;
+    } else {
+        //停止
+        Module1Status = STOP_STATUS;
+        Module2Status = STOP_STATUS;
+        RobotStatus   = STOP_STATUS;
+    }
+}
+
+
+
+/*  CAN1送信関数  */
+void CAN1_Trans(CANMessage *msg)
+{
+    can1.write(*msg);
+    wait(0.0020);
+//    255bit/500kbps
+//    wait(0.0051);
+//    20bit/500kbps
+//    wait_us(40);
+}
+
+/* CAN1受信関数 */
+void CAN1_Rcv(void)
+{
+    can1.read(Rcv_Msg);
+    if( Rcv_Msg.id == 0x7DF ) {
+        if( Rcv_Msg.data[0] == 0x01 ) {
+//            Module1Encoder = 0;
+            Module1Encoder += ( Rcv_Msg.data[1] << 8 );
+            Module1Encoder +=   Rcv_Msg.data[2];
+            Module1.Set_EncoderPulse( Module1Encoder );
+        } else if( Rcv_Msg.data[0] == 0x03 ) {
+//            Module2Encoder = 0;
+            Module2Encoder += ( Rcv_Msg.data[1] << 8 );
+            Module2Encoder +=   Rcv_Msg.data[2];
+            Module2.Set_EncoderPulse( Module2Encoder );
+        } else if( Rcv_Msg.data[0] == 0x06 ) {
+            LiftEncoder += ( Rcv_Msg.data[1] << 8 );
+            LiftEncoder +=   Rcv_Msg.data[2];
+        }
+    }
+}
+
+/* GUI受信割込み関数 */
+void GUI_Receive(void)
+{
+    GUIData = RaspPi.getc();
+//    if( GUIData != 0x00 && timer.read_ms() <= 200) {
+    if( GUIData != 0x00 ) {
+        Ps3_Led = 0;
+        Gui_Led = 1;
+        GUI_Safety_Timer.attach(&GUI_Safety_Stop, 0.1);
+        if(GUIData == 0x7F) {
+            EmgFlag = true;
+            RaspPi.printf("EMG:True");
+        } else if(GUIData == 0x80) {
+            EmgFlag = false;
+            RaspPi.printf("EMG:False");
+        }
+        ControlMode = GUIMODE;
+        RaspPi.printf("GUImode");
+    } else {
+        Ps3_Led = 1;
+        Gui_Led = 0;
+        GUI_Safety_Timer.detach();
+        ControlMode = PS3MODE;
+        Module1Status = MANUAL_STATUS;
+        Module2Status = MANUAL_STATUS;
+        RobotStatus   = MANUAL_STATUS;
+        RaspPi.printf("PS3mode ");
+    }
+    RaspPi.printf("GUIData:0x%x\n",GUIData);
+}
+
+
+/* GUIタイムアウト停止 */
+void GUI_Safety_Stop(void)
+{
+    //未実装
+}
+
+/* 非常停止ブザー制御関数 */
+void Buzzer_Toggle(void)
+{
+    if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) {
+        buzzer = !buzzer;
+    } else {
+        buzzer = 0;
+    }
+}
+
+/*  非常停止判定関数  */
+void Emg_Stop(void)
+{
+    Start = ps3.getSTARTState();
+    Select = ps3.getSELECTState();
+    if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) {
+        //停止
+        Emg_Led = 1;
+        emg_stop_tri = 0;
+    } else if( ( Start == 1 || EmgFlag == false) && ( emg_stop_sw == 0 ) ) {
+        //通常
+        Emg_Led = 0;
+        emg_stop_tri = 1;
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 26 00:35:42 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file