Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN

Dependencies:   mbed TrackedVehicle PS3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "TrackedVehicle.h"
00003 #include "Robot_Definition.h"
00004 #include "PS3.h"
00005 
00006 void Update_Robot_Status(void);
00007 void Lift_Control(void);
00008 void Paint_Control(void);
00009 void Cable_Control(void);
00010 void Outrigger_Control(void);
00011 void GUI_to_Status(void);
00012 void CAN1_Trans(CANMessage *msg);
00013 void CAN1_Rcv(void);
00014 void GUI_Receive(void);
00015 void GUI_Safety_Stop(void);
00016 void Buzzer_Toggle(void);
00017 void Emg_Stop(void);
00018 
00019 
00020 char GUIData = 0x00;
00021 char ResetData[8]   = {0, 0, BREAK_DUTY, 0,0,0,0,0};
00022 char MD5Data[8]   = {0, 5, BREAK_DUTY, 0,0,0,0,0};
00023 char MD6Data[8]   = {0, 6, BREAK_DUTY, 0,0,0,0,0};
00024 char MD7Data[8]   = {0, 7, BREAK_DUTY, 0,0,0,0,0};
00025 char MD8Data[8]   = {0, 8, BREAK_DUTY, 0,0,0,0,0};
00026 char MD9Data[8]   = {0, 9, BREAK_DUTY, 0,0,0,0,0};
00027 char MD10Data[8]   = {0, 10, BREAK_DUTY, 0,0,0,0,0};
00028 int8_t RightJoyX    = 0;
00029 int8_t RightJoyY    = 0;
00030 int8_t LeftJoyX     = 0;
00031 int8_t LeftJoyY     = 0;
00032 uint16_t Module1Encoder = 4096;
00033 uint16_t Module2Encoder = 4096;
00034 uint16_t LiftEncoder = 2400;
00035 short ControlMode       = PS3MODE;
00036 short RobotStatus       = INITIAL_STATUS;
00037 short Old_RobotStatus   = INITIAL_STATUS;
00038 short Module1Status     = INITIAL_STATUS;
00039 short Module2Status     = INITIAL_STATUS;
00040 
00041 bool MaintainAngle               = false;//sita
00042 bool ZeroPointTurn               = false;//ue
00043 bool RightTurn                   = false;//migi
00044 bool LeftTurn                    = false;//hidari
00045 bool CWTurn                      = false;//R1
00046 bool CCWTurn                     = false;//L1
00047 bool OppositePhase_RightTurn     = false;//maru
00048 bool OppositePhase_LeftTurn      = false;//sikaku
00049 bool Right2                      = false;//R2
00050 bool Left2                       = false;//L2
00051 bool LiftUp                      = false;//sankaku
00052 bool LiftDown                    = false;//batsu
00053 bool Old_MaintainAngle           = false;
00054 bool Old_ZeroPointTurn           = false;
00055 bool Old_RightTurn               = false;
00056 bool Old_LeftTurn                = false;
00057 bool Old_CWTurn                  = false;
00058 bool Old_CCWTurn                 = false;
00059 bool Old_OppositePhase_RightTurn = false;
00060 bool Old_OppositePhase_LeftTurn  = false;
00061 bool Old_LiftUp                  = false;
00062 bool Old_LiftDown                = false;
00063 
00064 bool EmgFlag = true;
00065 bool Start = false;
00066 bool Select = false;
00067 bool Old_Start = false;
00068 bool Old_Select = false;
00069 bool Old_emg_stop_sw = false;
00070 
00071 CANMessage Rcv_Msg;
00072 CANMessage MD1_Msg(0x7E0,ResetData,3,CANData,CANStandard);//Module1Right
00073 CANMessage MD2_Msg(0x7E1,ResetData,3,CANData,CANStandard);//Module1Left
00074 CANMessage MD3_Msg(0x7E2,ResetData,3,CANData,CANStandard);//Module2Right
00075 CANMessage MD4_Msg(0x7E3,ResetData,3,CANData,CANStandard);//Module2Left
00076 CANMessage MD5_Msg(0x7E4,MD5Data,3,CANData,CANStandard);//lift
00077 CANMessage MD6_Msg(0x7E5,MD6Data,3,CANData,CANStandard);//X
00078 CANMessage MD7_Msg(0x7E6,MD7Data,3,CANData,CANStandard);//Y
00079 CANMessage MD8_Msg(0x7E7,MD8Data,3,CANData,CANStandard);//Z
00080 CANMessage MD9_Msg(0x7E8,MD9Data,3,CANData,CANStandard);//Cable
00081 CANMessage MD10_Msg(0x7E9,MD10Data,3,CANData,CANStandard);//Outrigger
00082 
00083 Ticker GUI_Safety_Timer;
00084 Ticker EMG_Buzzer_Timer;
00085 CAN can1(PA_11, PA_12,500000);
00086 Serial pc(USBTX,USBRX,115200);
00087 Serial RaspPi(PC_12, PD_2,115200);
00088 
00089 PS3 ps3(PC_10,PC_11);
00090 TrackedVehicle Module1(0x7E0, 0x7E1);
00091 TrackedVehicle Module2(0x7E2, 0x7E3);
00092 
00093 DigitalOut led1(LED1);
00094 DigitalOut emg_stop_tri(PB_14);
00095 DigitalOut Ps3_Led(PC_0);
00096 DigitalOut Pwr_Led(PC_1);
00097 DigitalOut Emg_Led(PB_0);
00098 DigitalOut Gui_Led(PA_4);
00099 DigitalOut buzzer(PA_1);
00100 DigitalIn  emg_stop_sw(PB_13);
00101 
00102 int main()
00103 {
00104     emg_stop_tri = 0;
00105     Ps3_Led = 0;
00106     Pwr_Led = 1;
00107     Emg_Led = 1;
00108     Gui_Led = 1;
00109     emg_stop_sw.mode( PullUp );
00110 //    コメント外すとうるさいよ
00111 //    EMG_Buzzer_Timer.attach(&Buzzer_Toggle, 1.5);
00112     can1.attach(&CAN1_Rcv,CAN::RxIrq);
00113 //    pc.attach(&GUI_Receive,Serial::RxIrq);
00114     RaspPi.attach(&GUI_Receive,Serial::RxIrq);
00115     CAN1_Trans(&MD1_Msg);
00116     CAN1_Trans(&MD2_Msg);
00117     CAN1_Trans(&MD3_Msg);
00118     CAN1_Trans(&MD4_Msg);
00119     CAN1_Trans(&MD5_Msg);
00120     CAN1_Trans(&MD6_Msg);
00121     CAN1_Trans(&MD7_Msg);
00122     CAN1_Trans(&MD8_Msg);
00123     CAN1_Trans(&MD9_Msg);
00124     CAN1_Trans(&MD10_Msg);
00125 //    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET);
00126     while(1) {
00127         Emg_Stop();
00128         if(ControlMode == PS3MODE) {
00129             Update_Robot_Status();
00130         } else if(ControlMode == GUIMODE) {
00131             GUI_to_Status();
00132         }
00133         Lift_Control();
00134         Paint_Control();
00135         Cable_Control();
00136         Outrigger_Control();
00137         switch (RobotStatus) {
00138             case STOP_STATUS:
00139                 Module1.Stop_Robot();
00140                 Module2.Stop_Robot();
00141                 break;
00142             //1,2(前後のみ)
00143             case ZERO_POINT_TURN:
00144                 Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY);
00145                 Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY);
00146                 break;
00147 
00148             //3,4,5,6(左右緩旋回前後)
00149             case OPPOSITE_PHASE_RIGHT_TURN:
00150             case OPPOSITE_PHASE_LEFT_TURN:
00151             case RIGHT_SOFT_TURN:
00152             case LEFT_SOFT_TURN:
00153             case RIGHT_SOFT_TURN_BACK:
00154             case LEFT_SOFT_TURN_BACK:
00155                 Module1.Control_Turn(Module1Status);
00156                 Module2.Control_Turn(Module2Status);
00157                 break;
00158 
00159             //7,8(90°旋回と前後)
00160             case RIGHT90_TURN:
00161             case LEFT90_TURN:
00162                 Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY);
00163                 Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY);
00164                 break;
00165 
00166             //9,10(全体CW,CW)
00167             case ROBOT_CW_TURN:
00168             case ROBOT_CCW_TURN:
00169                 Module1.Control_Robot_Turn(Module1Status, RobotStatus);
00170                 Module2.Control_Robot_Turn(Module2Status, RobotStatus);
00171                 break;
00172 
00173             //ワカラン
00174             case INITIAL_STATUS:
00175                 Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY);
00176                 Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY);
00177                 break;
00178 
00179             //マニュアル操作
00180             case MANUAL_STATUS:
00181                 Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY);
00182                 Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY);
00183                 break;
00184         }
00185         CAN1_Trans(Module1.Get_RightMsg());
00186         CAN1_Trans(Module1.Get_LeftMsg());
00187         CAN1_Trans(Module2.Get_RightMsg());
00188         CAN1_Trans(Module2.Get_LeftMsg());
00189         CAN1_Trans(&MD5_Msg);
00190         CAN1_Trans(&MD6_Msg);
00191         CAN1_Trans(&MD7_Msg);
00192         CAN1_Trans(&MD8_Msg);
00193         CAN1_Trans(&MD9_Msg);
00194         CAN1_Trans(&MD10_Msg);
00195         pc.printf("Enc1:%d,Enc2:%d",Module1.Get_ModulePulse(),Module2.Get_ModulePulse());
00196         pc.printf("GUIData:0x%x\n",GUIData);
00197     }
00198 }
00199 /* ここまでmain文 */
00200 
00201 /* PS3 -> ロボットステータス更新関数 */
00202 void Update_Robot_Status(void)
00203 {
00204     MaintainAngle           = ps3.getButtonState( sita );
00205     ZeroPointTurn           = ps3.getButtonState( ue );
00206     RightTurn               = ps3.getButtonState( migi );
00207     LeftTurn                = ps3.getButtonState( hidari );
00208     CWTurn                  = ps3.getButtonState( R1 );
00209     CCWTurn                 = ps3.getButtonState( L1 );
00210     Right2                  = ps3.getButtonState( R2 );
00211     Left2                   = ps3.getButtonState( L2 );
00212     OppositePhase_RightTurn = ps3.getButtonState( maru );
00213     OppositePhase_LeftTurn  = ps3.getButtonState( sikaku );
00214     RightJoyY               = ps3.getRightJoystickYaxis();
00215     LeftJoyY                = ps3.getLeftJoystickYaxis();
00216 
00217     if( Start != true && Select != true) {
00218         if( (ZeroPointTurn == true) && (Old_ZeroPointTurn == false) ) {
00219             //正面
00220             Module1.Set_TargetPulse(4096);
00221             Module2.Set_TargetPulse(4096);
00222             Module1Status = ZERO_POINT_TURN;
00223             Module2Status = ZERO_POINT_TURN;
00224             RobotStatus   = ZERO_POINT_TURN;
00225         } else if( (RightTurn == true) && (Old_RightTurn == false) ) {
00226             //右旋回90°
00227             Module1.Set_TargetPulse(6144);
00228             Module2.Set_TargetPulse(6144);
00229             Module1Status = RIGHT90_TURN;
00230             Module2Status = RIGHT90_TURN;
00231             RobotStatus   = RIGHT90_TURN;
00232 //        } else if( (LeftTurn == true) && (Old_LeftTurn == false) ) {
00233 //            //左旋回90°
00234 //            Module1.Set_TargetPulse(2048);
00235 //            Module2.Set_TargetPulse(2048);
00236 //            Module1Status = LEFT90_TURN;
00237 //            Module2Status = LEFT90_TURN;
00238 //            RobotStatus   = LEFT90_TURN;
00239         } else if( (OppositePhase_RightTurn == true) && (Old_OppositePhase_RightTurn == false) ) {
00240             //逆位相右旋回±30°
00241             Module1.Set_TargetPulse(4778);
00242             Module2.Set_TargetPulse(3414);
00243             Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
00244             Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
00245             RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
00246         } else if( (OppositePhase_LeftTurn == true) && (Old_OppositePhase_LeftTurn == false) ) {
00247             //逆位相左旋回±30°
00248             Module1.Set_TargetPulse(3414);
00249             Module2.Set_TargetPulse(4778);
00250             Module1Status = OPPOSITE_PHASE_LEFT_TURN;
00251             Module2Status = OPPOSITE_PHASE_LEFT_TURN;
00252             RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
00253         } else if( LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) {
00254             //全体右緩旋回
00255             Module1.Set_TargetPulse(4778);
00256             Module2.Set_TargetPulse(3414);
00257             Module1Status = RIGHT_SOFT_TURN;
00258             Module2Status = RIGHT_SOFT_TURN;
00259             RobotStatus   = RIGHT_SOFT_TURN;
00260         } else if( LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) {
00261             //全体後ろ右緩旋回
00262             Module1.Set_TargetPulse(4778);
00263             Module2.Set_TargetPulse(3414);
00264             Module1Status = RIGHT_SOFT_TURN_BACK;
00265             Module2Status = RIGHT_SOFT_TURN_BACK;
00266             RobotStatus   = RIGHT_SOFT_TURN_BACK;
00267         } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == RIGHT_SOFT_TURN || RobotStatus == RIGHT_SOFT_TURN_BACK) ) {
00268             //全体右緩旋回停止->逆位相右旋回±30°
00269             Module1.Set_TargetPulse(4778);
00270             Module2.Set_TargetPulse(3414);
00271             Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
00272             Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
00273             RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
00274         } else if(LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) {
00275             //全体左緩旋回
00276             Module1.Set_TargetPulse(3414);
00277             Module2.Set_TargetPulse(4778);
00278             Module1Status = LEFT_SOFT_TURN;
00279             Module2Status = LEFT_SOFT_TURN;
00280             RobotStatus   = LEFT_SOFT_TURN;
00281         } else if(LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) {
00282             //全体後ろ左緩旋回
00283             Module1.Set_TargetPulse(3414);
00284             Module2.Set_TargetPulse(4778);
00285             Module1Status = LEFT_SOFT_TURN_BACK;
00286             Module2Status = LEFT_SOFT_TURN_BACK;
00287             RobotStatus   = LEFT_SOFT_TURN_BACK;
00288         } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == LEFT_SOFT_TURN || RobotStatus == LEFT_SOFT_TURN_BACK) ) {
00289             //全体左緩旋回停止->逆位相左旋回±45°
00290             Module1.Set_TargetPulse(3414);
00291             Module2.Set_TargetPulse(4778);
00292             Module1Status = OPPOSITE_PHASE_LEFT_TURN;
00293             Module2Status = OPPOSITE_PHASE_LEFT_TURN;
00294             RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
00295         } else if( (CWTurn == true) && (RobotStatus == RIGHT90_TURN) ) {
00296             //全体CW
00297             Module1.Set_TargetPulse(6144);
00298             Module2.Set_TargetPulse(6144);
00299             Module1Status = MODULE_FRONT;
00300             Module2Status = MODULE_BACK;
00301             RobotStatus   = ROBOT_CW_TURN;
00302         } else if( (CCWTurn == true) && (RobotStatus == RIGHT90_TURN) ) {
00303             //全体CCW
00304             Module1.Set_TargetPulse(6144);
00305             Module2.Set_TargetPulse(6144);
00306             Module1Status = MODULE_BACK;
00307             Module2Status = MODULE_FRONT;
00308             RobotStatus   = ROBOT_CCW_TURN;
00309         } else if( (CWTurn != true && CCWTurn != true) && (RobotStatus == ROBOT_CW_TURN || RobotStatus == ROBOT_CCW_TURN) ) {
00310             //全体左右超信地旋回 -> 右旋回90°
00311             Module1.Set_TargetPulse(6144);
00312             Module2.Set_TargetPulse(6144);
00313             Module1Status = RIGHT90_TURN;
00314             Module2Status = RIGHT90_TURN;
00315             RobotStatus   = RIGHT90_TURN;
00316         } else if( (MaintainAngle == true) && (Old_MaintainAngle == false) ) {
00317             //十字ボタン下でその角度維持
00318             Module1.Set_TargetPulse(Module1.Get_ModulePulse());
00319             Module2.Set_TargetPulse(Module2.Get_ModulePulse());
00320             Module1Status = INITIAL_STATUS;
00321             Module2Status = INITIAL_STATUS;
00322             RobotStatus   = INITIAL_STATUS;
00323         }
00324     } else if( (Start == true) && (Old_Start == false) ) {
00325         Module1Status = MANUAL_STATUS;
00326         Module2Status = MANUAL_STATUS;
00327         RobotStatus   = MANUAL_STATUS;
00328     }
00329     Old_MaintainAngle           = MaintainAngle;
00330     Old_ZeroPointTurn           = ZeroPointTurn;
00331     Old_RightTurn               = RightTurn;
00332     Old_LeftTurn                = LeftTurn;
00333     Old_CWTurn                  = CWTurn;
00334     Old_CCWTurn                 = CCWTurn;
00335     Old_OppositePhase_RightTurn = OppositePhase_RightTurn;
00336     Old_OppositePhase_LeftTurn  = OppositePhase_LeftTurn;
00337     Old_Start                   = Start;
00338 }
00339 
00340 /* Liftの処理関数 */
00341 void Lift_Control(void)
00342 {
00343     LiftUp   = ps3.getButtonState( sankaku );
00344     LiftDown = ps3.getButtonState( batu );
00345     if( LiftUp == true || GUIData == 0x0C ) {
00346         //LIFT UP
00347         MD5_Msg.data[2] = CW50_DUTY;
00348     } else if( LiftDown == true || GUIData == 0x0D ) {
00349         //LIFT DOWN
00350         MD5_Msg.data[2] = CCW50_DUTY;
00351     } else {
00352         MD5_Msg.data[2] = BREAK_DUTY;
00353     }
00354 }
00355 
00356 /* Paintの処理関数 */
00357 void Paint_Control(void)
00358 {
00359     MD6_Msg.data[2] = BREAK_DUTY;
00360     MD7_Msg.data[2] = BREAK_DUTY;
00361     MD8_Msg.data[2] = BREAK_DUTY;
00362     if(GUIData == 0x0F) {
00363         //X_Right
00364         MD6_Msg.data[2] = CW50_DUTY;
00365     } else if(GUIData == 0x10) {
00366         //X_Left
00367         MD6_Msg.data[2] = CCW50_DUTY;
00368     } else if(GUIData == 0x11) {
00369         //Y_Up
00370         MD7_Msg.data[2] = CCW50_DUTY;
00371     } else if(GUIData == 0x12) {
00372         //Y_Down
00373         MD7_Msg.data[2] = CW50_DUTY;
00374     } else if(GUIData == 0x13) {
00375         //Z_Push
00376         MD8_Msg.data[2] = CCW50_DUTY;
00377     } else if(GUIData == 0x14) {
00378         //Z_Pull
00379         MD8_Msg.data[2] = CW50_DUTY;
00380     } else {
00381         MD6_Msg.data[2] = BREAK_DUTY;
00382         MD7_Msg.data[2] = BREAK_DUTY;
00383         MD8_Msg.data[2] = BREAK_DUTY;
00384     }
00385 }
00386 /* Cableの処理関数 */
00387 void Cable_Control(void)
00388 {
00389 //    if(GUIData == 0x16) {
00390     if(GUIData == 0x02 || GUIData == 0x04 || GUIData == 0x06 || GUIData == 0x08 || GUIData == 0x16) {
00391         //Cable収納
00392         MD9_Msg.data[2] = CW50_DUTY;
00393 //    } else if(GUIData == 0x17) {
00394     } else if(GUIData == 0x01 || GUIData == 0x03 || GUIData == 0x05 || GUIData == 0x07 || GUIData == 0x17) {
00395         //Cable排出
00396         MD9_Msg.data[2] = CCW50_DUTY;
00397     } else {
00398         MD9_Msg.data[2] = BREAK_DUTY;
00399     }
00400 }
00401 
00402 /* Outriggerの処理関数 */
00403 void Outrigger_Control(void)
00404 {
00405     if(GUIData == 0x19) {
00406         //Outrigger展開
00407         MD10_Msg.data[2] = CW20_DUTY;
00408     } else if(GUIData == 0x1A) {
00409         //Outrigger収納
00410         MD10_Msg.data[2] = CCW20_DUTY;
00411     } else {
00412         MD10_Msg.data[2] = BREAK_DUTY;
00413     }
00414 }
00415 
00416 /* GUI -> Status変換関数 */
00417 void GUI_to_Status(void)
00418 {
00419     LeftJoyY = 0;
00420     if( GUIData == 0x01 ) {
00421         //前進
00422         Module1.Set_TargetPulse(4096);
00423         Module2.Set_TargetPulse(4096);
00424         Module1Status = ZERO_POINT_TURN;
00425         Module2Status = ZERO_POINT_TURN;
00426         RobotStatus   = ZERO_POINT_TURN;
00427         LeftJoyY = 63;
00428     } else if( GUIData == 0x02 ) {
00429         //後進
00430         Module1.Set_TargetPulse(4096);
00431         Module2.Set_TargetPulse(4096);
00432         Module1Status = ZERO_POINT_TURN;
00433         Module2Status = ZERO_POINT_TURN;
00434         RobotStatus   = ZERO_POINT_TURN;
00435         LeftJoyY = -63;
00436     } else if( GUIData == 0x03 ) {
00437         //右緩旋回
00438         Module1.Set_TargetPulse(4778);
00439         Module2.Set_TargetPulse(3414);
00440         Module1Status = RIGHT_SOFT_TURN;
00441         Module2Status = RIGHT_SOFT_TURN;
00442         RobotStatus   = RIGHT_SOFT_TURN;
00443     } else if( GUIData == 0x04 ) {
00444         //右緩後旋回
00445         Module1.Set_TargetPulse(4778);
00446         Module2.Set_TargetPulse(3414);
00447         Module1Status = RIGHT_SOFT_TURN_BACK;
00448         Module2Status = RIGHT_SOFT_TURN_BACK;
00449         RobotStatus   = RIGHT_SOFT_TURN_BACK;
00450     } else if( GUIData == 0x05 ) {
00451         //左緩旋回
00452         Module1.Set_TargetPulse(3414);
00453         Module2.Set_TargetPulse(4778);
00454         Module1Status = LEFT_SOFT_TURN;
00455         Module2Status = LEFT_SOFT_TURN;
00456         RobotStatus   = LEFT_SOFT_TURN;
00457     } else if( GUIData == 0x06 ) {
00458         //左緩後旋回
00459         Module1.Set_TargetPulse(3414);
00460         Module2.Set_TargetPulse(4778);
00461         Module1Status = LEFT_SOFT_TURN_BACK;
00462         Module2Status = LEFT_SOFT_TURN_BACK;
00463         RobotStatus   = LEFT_SOFT_TURN_BACK;
00464     } else if( GUIData == 0x07 ) {
00465         //右向き前進
00466         Module1.Set_TargetPulse(6144);
00467         Module2.Set_TargetPulse(6144);
00468         Module1Status = RIGHT90_TURN;
00469         Module2Status = RIGHT90_TURN;
00470         RobotStatus   = RIGHT90_TURN;
00471         LeftJoyY = 63;
00472     } else if( GUIData == 0x08 ) {
00473         //右向き後進
00474         Module1.Set_TargetPulse(6144);
00475         Module2.Set_TargetPulse(6144);
00476         Module1Status = RIGHT90_TURN;
00477         Module2Status = RIGHT90_TURN;
00478         RobotStatus   = RIGHT90_TURN;
00479         LeftJoyY = -63;
00480     } else if( GUIData == 0x09 ) {
00481         //全体CW
00482         Module1.Set_TargetPulse(6144);
00483         Module2.Set_TargetPulse(6144);
00484         Module1Status = MODULE_FRONT;
00485         Module2Status = MODULE_BACK;
00486         RobotStatus   = ROBOT_CW_TURN;
00487     } else if( GUIData == 0x0A ) {
00488         //全体CCW
00489         Module1.Set_TargetPulse(6144);
00490         Module2.Set_TargetPulse(6144);
00491         Module1Status = MODULE_BACK;
00492         Module2Status = MODULE_FRONT;
00493         RobotStatus   = ROBOT_CCW_TURN;
00494     } else if( GUIData == 0xFF ) {
00495         //0°(正面)
00496         Module1.Set_TargetPulse(4096);
00497         Module2.Set_TargetPulse(4096);
00498         Module1Status = ZERO_POINT_TURN;
00499         Module2Status = ZERO_POINT_TURN;
00500         RobotStatus   = ZERO_POINT_TURN;
00501     } else if( GUIData == 0xFE ) {
00502         //±30°(右緩旋回)
00503         Module1.Set_TargetPulse(4778);
00504         Module2.Set_TargetPulse(3414);
00505         Module1Status = OPPOSITE_PHASE_RIGHT_TURN;
00506         Module2Status = OPPOSITE_PHASE_RIGHT_TURN;
00507         RobotStatus   = OPPOSITE_PHASE_RIGHT_TURN;
00508     } else if( GUIData == 0xFD ) {
00509         //±30°(左緩旋回)
00510         Module1.Set_TargetPulse(3414);
00511         Module2.Set_TargetPulse(4778);
00512         Module1Status = OPPOSITE_PHASE_LEFT_TURN;
00513         Module2Status = OPPOSITE_PHASE_LEFT_TURN;
00514         RobotStatus   = OPPOSITE_PHASE_LEFT_TURN;
00515     } else if( GUIData == 0xFC ) {
00516         //90°(右向き)
00517         Module1.Set_TargetPulse(6144);
00518         Module2.Set_TargetPulse(6144);
00519         Module1Status = RIGHT90_TURN;
00520         Module2Status = RIGHT90_TURN;
00521         RobotStatus   = RIGHT90_TURN;
00522     } else {
00523         //停止
00524         Module1Status = STOP_STATUS;
00525         Module2Status = STOP_STATUS;
00526         RobotStatus   = STOP_STATUS;
00527     }
00528 }
00529 
00530 
00531 
00532 /*  CAN1送信関数  */
00533 void CAN1_Trans(CANMessage *msg)
00534 {
00535     can1.write(*msg);
00536     wait(0.0020);
00537 //    255bit/500kbps
00538 //    wait(0.0051);
00539 //    20bit/500kbps
00540 //    wait_us(40);
00541 }
00542 
00543 /* CAN1受信関数 */
00544 void CAN1_Rcv(void)
00545 {
00546     can1.read(Rcv_Msg);
00547     if( Rcv_Msg.id == 0x7DF ) {
00548         if( Rcv_Msg.data[0] == 0x01 || Rcv_Msg.data[0] == 0x02 ) {
00549             Module1Encoder = 0;
00550             Module1Encoder += ( Rcv_Msg.data[1] << 8 );
00551             Module1Encoder +=   Rcv_Msg.data[2];
00552             Module1.Set_EncoderPulse( Module1Encoder );
00553         } else if( Rcv_Msg.data[0] == 0x03 || Rcv_Msg.data[0] == 0x04 ) {
00554             Module2Encoder = 0;
00555             Module2Encoder += ( Rcv_Msg.data[1] << 8 );
00556             Module2Encoder +=   Rcv_Msg.data[2];
00557             Module2.Set_EncoderPulse( Module2Encoder );
00558         } else if( Rcv_Msg.data[0] == 0x06 ) {
00559             LiftEncoder += ( Rcv_Msg.data[1] << 8 );
00560             LiftEncoder +=   Rcv_Msg.data[2];
00561         }
00562     }
00563 }
00564 
00565 /* GUI受信割込み関数 */
00566 void GUI_Receive(void)
00567 {
00568     GUIData = RaspPi.getc();
00569 //    if( GUIData != 0x00 && timer.read_ms() <= 200) {
00570     if( GUIData != 0x00 ) {
00571         Ps3_Led = 0;
00572         Gui_Led = 1;
00573         GUI_Safety_Timer.attach(&GUI_Safety_Stop, 0.1);
00574         if(GUIData == 0x7F) {
00575             EmgFlag = true;
00576             RaspPi.printf("EMG:True");
00577         } else if(GUIData == 0x80) {
00578             EmgFlag = false;
00579             RaspPi.printf("EMG:False");
00580         }
00581         ControlMode = GUIMODE;
00582         RaspPi.printf("GUImode");
00583     } else {
00584         Ps3_Led = 1;
00585         Gui_Led = 0;
00586         GUI_Safety_Timer.detach();
00587         ControlMode = PS3MODE;
00588         Module1Status = MANUAL_STATUS;
00589         Module2Status = MANUAL_STATUS;
00590         RobotStatus   = MANUAL_STATUS;
00591         RaspPi.printf("PS3mode ");
00592     }
00593     RaspPi.printf("GUIData:0x%x\n",GUIData);
00594 }
00595 
00596 
00597 /* GUIタイムアウト停止 */
00598 void GUI_Safety_Stop(void)
00599 {
00600     //未実装
00601 }
00602 
00603 /* 非常停止ブザー制御関数 */
00604 void Buzzer_Toggle(void)
00605 {
00606     if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) {
00607         buzzer = !buzzer;
00608     } else {
00609         buzzer = 0;
00610     }
00611 }
00612 
00613 /*  非常停止判定関数  */
00614 void Emg_Stop(void)
00615 {
00616     Start = ps3.getSTARTState();
00617     Select = ps3.getSELECTState();
00618     if(Select == true) {
00619         //停止
00620         Emg_Led = 1;
00621         emg_stop_tri = 0;
00622         EmgFlag = true;
00623     } else if(Start == true && emg_stop_sw == 0 ) {
00624         //通常
00625         Emg_Led = 0;
00626         emg_stop_tri = 1;
00627         EmgFlag = false;
00628     }
00629 
00630     if( EmgFlag == true || emg_stop_sw == 1 ) {
00631         //停止
00632         Emg_Led = 1;
00633         emg_stop_tri = 0;
00634     } else if( EmgFlag == false && emg_stop_sw == 0 ) {
00635         //通常
00636         Emg_Led = 0;
00637         emg_stop_tri = 1;
00638     }
00639 }