Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN

Dependencies:   mbed TrackedVehicle PS3

Files at this revision

API Documentation at this revision

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;