Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN
Dependencies: mbed TrackedVehicle PS3
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 }
Generated on Thu Jul 28 2022 11:28:20 by
1.7.2