Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN
Dependencies: mbed TrackedVehicle PS3
Revision 2:8b32f6901e10, committed 2021-12-05
- Comitter:
- st17099ng
- Date:
- Sun Dec 05 01:30:29 2021 +0000
- Parent:
- 1:ca42cf6ae77e
- Commit message:
- 12/5; back up commit.
Changed in this revision
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 |
--- a/TrackedVehicle.lib Fri Nov 26 01:18:46 2021 +0000 +++ b/TrackedVehicle.lib Sun Dec 05 01:30:29 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/st17099ng/code/TrackedVehicle/#b5561184f201 +https://os.mbed.com/users/st17099ng/code/TrackedVehicle/#132886173bec
--- a/main.cpp Fri Nov 26 01:18:46 2021 +0000 +++ b/main.cpp Sun Dec 05 01:30:29 2021 +0000 @@ -32,7 +32,7 @@ uint16_t Module1Encoder = 4096; uint16_t Module2Encoder = 4096; uint16_t LiftEncoder = 2400; -short ControlMode = GUIMODE; +short ControlMode = PS3MODE; short RobotStatus = INITIAL_STATUS; short Old_RobotStatus = INITIAL_STATUS; short Module1Status = INITIAL_STATUS; @@ -68,6 +68,18 @@ bool Old_Select = false; bool Old_emg_stop_sw = false; +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 + Ticker GUI_Safety_Timer; Ticker EMG_Buzzer_Timer; CAN can1(PA_11, PA_12,500000); @@ -87,18 +99,6 @@ 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; @@ -122,7 +122,7 @@ CAN1_Trans(&MD8_Msg); CAN1_Trans(&MD9_Msg); CAN1_Trans(&MD10_Msg); - HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET); +// HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET); while(1) { Emg_Stop(); if(ControlMode == PS3MODE) { @@ -192,6 +192,8 @@ CAN1_Trans(&MD8_Msg); CAN1_Trans(&MD9_Msg); CAN1_Trans(&MD10_Msg); + pc.printf("Enc1:%d,Enc2:%d",Module1.Get_ModulePulse(),Module2.Get_ModulePulse()); + pc.printf("GUIData:0x%x\n",GUIData); } } /* ここまでmain文 */ @@ -358,17 +360,23 @@ MD7_Msg.data[2] = BREAK_DUTY; MD8_Msg.data[2] = BREAK_DUTY; if(GUIData == 0x0F) { + //X_Right MD6_Msg.data[2] = CW50_DUTY; } else if(GUIData == 0x10) { + //X_Left MD6_Msg.data[2] = CCW50_DUTY; } else if(GUIData == 0x11) { - MD7_Msg.data[2] = CW50_DUTY; + //Y_Up + MD7_Msg.data[2] = CCW50_DUTY; } else if(GUIData == 0x12) { - MD7_Msg.data[2] = CCW50_DUTY; + //Y_Down + MD7_Msg.data[2] = CW50_DUTY; } else if(GUIData == 0x13) { - MD8_Msg.data[2] = CW50_DUTY; + //Z_Push + MD8_Msg.data[2] = CCW50_DUTY; } else if(GUIData == 0x14) { - MD8_Msg.data[2] = CCW50_DUTY; + //Z_Pull + MD8_Msg.data[2] = CW50_DUTY; } else { MD6_Msg.data[2] = BREAK_DUTY; MD7_Msg.data[2] = BREAK_DUTY; @@ -378,10 +386,12 @@ /* Cableの処理関数 */ void Cable_Control(void) { - if(GUIData == 0x16) { +// if(GUIData == 0x16) { + if(GUIData == 0x02 || GUIData == 0x04 || GUIData == 0x06 || GUIData == 0x08 || GUIData == 0x16) { //Cable収納 MD9_Msg.data[2] = CW50_DUTY; - } else if(GUIData == 0x17) { +// } else if(GUIData == 0x17) { + } else if(GUIData == 0x01 || GUIData == 0x03 || GUIData == 0x05 || GUIData == 0x07 || GUIData == 0x17) { //Cable排出 MD9_Msg.data[2] = CCW50_DUTY; } else { @@ -394,10 +404,10 @@ { if(GUIData == 0x19) { //Outrigger展開 - MD10_Msg.data[2] = CW50_DUTY; + MD10_Msg.data[2] = CW20_DUTY; } else if(GUIData == 0x1A) { //Outrigger収納 - MD10_Msg.data[2] = CCW50_DUTY; + MD10_Msg.data[2] = CCW20_DUTY; } else { MD10_Msg.data[2] = BREAK_DUTY; } @@ -535,13 +545,13 @@ { can1.read(Rcv_Msg); if( Rcv_Msg.id == 0x7DF ) { - if( Rcv_Msg.data[0] == 0x01 ) { -// Module1Encoder = 0; + if( Rcv_Msg.data[0] == 0x01 || Rcv_Msg.data[0] == 0x02 ) { + 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; + } else if( Rcv_Msg.data[0] == 0x03 || Rcv_Msg.data[0] == 0x04 ) { + Module2Encoder = 0; Module2Encoder += ( Rcv_Msg.data[1] << 8 ); Module2Encoder += Rcv_Msg.data[2]; Module2.Set_EncoderPulse( Module2Encoder ); @@ -605,11 +615,23 @@ { Start = ps3.getSTARTState(); Select = ps3.getSELECTState(); - if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) { + if(Select == true) { //停止 Emg_Led = 1; emg_stop_tri = 0; - } else if( ( Start == 1 || EmgFlag == false) && ( emg_stop_sw == 0 ) ) { + EmgFlag = true; + } else if(Start == true && emg_stop_sw == 0 ) { + //通常 + Emg_Led = 0; + emg_stop_tri = 1; + EmgFlag = false; + } + + if( EmgFlag == true || emg_stop_sw == 1 ) { + //停止 + Emg_Led = 1; + emg_stop_tri = 0; + } else if( EmgFlag == false && emg_stop_sw == 0 ) { //通常 Emg_Led = 0; emg_stop_tri = 1;