Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN

Dependencies:   mbed TrackedVehicle PS3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers README.h Source File

README.h

00001 /*************************************************************************************************
00002 
00003 program変更履歴
00004 --------------------------------------------------------------------
00005  2021/10/11 R.I.P.
00006  TrackedVehicleにConvert_PS3toMsgとして移行
00007  PS3Data -> Duty変換関数
00008 void Data_To_Duty(void)
00009 {
00010     bool r1 = ps3.getButtonState( R1 );
00011     bool l1 = ps3.getButtonState( L1 );
00012     bool r2 = ps3.getButtonState( R2 );
00013     bool l2 = ps3.getButtonState( L2 );
00014     MD1_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F;
00015     MD2_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F;
00016     MD3_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F;
00017     MD4_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F;
00018 
00019               モジュール1
00020       信地右旋回
00021     if( ( r1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) {
00022 
00023         MD1_Duty = BREAK_DUTY;
00024         MD2_Duty = HALF_CCW_DUTY;
00025 
00026           信地左旋回
00027     } else if( ( l1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) {
00028 
00029         MD1_Duty = HALF_CCW_DUTY;
00030         MD2_Duty = BREAK_DUTY;
00031 
00032           超信地右旋回
00033     } else if( r1 == 1) {
00034 
00035         MD1_Duty = HALF_CW_DUTY;
00036         MD2_Duty = HALF_CCW_DUTY;
00037 
00038           超信地左旋回
00039     } else if( l1 == 1 ) {
00040 
00041         MD1_Duty = HALF_CCW_DUTY;
00042         MD2_Duty = HALF_CW_DUTY;
00043 
00044           前進(JoyStickの閾値32の時
00045           計算式:DEC(32*2) + HEX(0x7F) = 0xBF
00046     } else if( MD1_MaxDuty >= 0xBF ) {
00047 
00048 //        MD1_Duty = CCW10_DUTY;
00049         MD1_Duty = HALF_CCW_DUTY;
00050         MD2_Duty = HALF_CCW_DUTY;
00051 
00052          後進(JoyStickの閾値-32の時
00053           計算式:DEC(-32*2) + HEX(0x7F) = 0x3F
00054     } else if( MD1_MaxDuty <= 0x3F ) {
00055 
00056         MD1_Duty = CW10_DUTY;
00057         MD1_Duty = HALF_CW_DUTY;
00058         MD2_Duty = HALF_CW_DUTY;
00059 
00060           停止
00061     } else {
00062 
00063         MD1_Duty = BREAK_DUTY;
00064         MD2_Duty = BREAK_DUTY;
00065 
00066     }
00067 
00068 
00069               モジュール2
00070       信地右旋回
00071     if( ( r2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) {
00072 
00073         MD3_Duty = BREAK_DUTY;
00074         MD4_Duty = HALF_CCW_DUTY;
00075 
00076           信地左旋回
00077     } else if( ( l2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) {
00078 
00079         MD3_Duty = HALF_CCW_DUTY;
00080         MD4_Duty = BREAK_DUTY;
00081 
00082           超信地右旋回
00083     } else if( r2 == 1) {
00084 
00085         MD3_Duty = HALF_CW_DUTY;
00086         MD4_Duty = HALF_CCW_DUTY;
00087 
00088           超信地左旋回
00089     } else if( l2 == 1 ) {
00090 
00091         MD3_Duty = HALF_CCW_DUTY;
00092         MD4_Duty = HALF_CW_DUTY;
00093 
00094           前進(JoyStickの閾値32の時
00095           計算式:DEC(32*2) + HEX(0x7F) = 0xBF
00096     } else if( MD3_MaxDuty >= 0xBF ) {
00097 
00098         MD3_Duty = HALF_CCW_DUTY;
00099         MD4_Duty = HALF_CCW_DUTY;
00100 
00101          後進(JoyStickの閾値-32の時
00102           計算式:DEC(-32*2) + HEX(0x7F) = 0x3F
00103     } else if( MD3_MaxDuty <= 0x3F ) {
00104 
00105         MD3_Duty = HALF_CW_DUTY;
00106         MD4_Duty = HALF_CW_DUTY;
00107 
00108           停止
00109     } else {
00110 
00111         MD3_Duty = BREAK_DUTY;
00112         MD4_Duty = BREAK_DUTY;
00113 
00114     }
00115 }
00116 ---台形制御をMDのプログラムに導入したため---
00117 ---台形制御関数---
00118 void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty)
00119 {
00120     int Diff_Duty = MD_Duty - msg->data[2];
00121     if ( msg->data[2] == MD_Duty ) {
00122 
00123         msg->data[2] = MD_Duty;
00124 
00125     } else if( Diff_Duty >= 3 ) {
00126 
00127         msg->data[2] += ADJUST_DUTY;
00128 
00129     } else if( ( Diff_Duty < 3 ) && ( Diff_Duty > 0 ) ) {
00130 
00131         msg->data[2] += Diff_Duty;
00132 
00133     } else if( ( Diff_Duty > -3 ) && ( Diff_Duty < 0 ) ) {
00134 
00135         msg->data[2] += Diff_Duty;
00136 
00137     } else if( Diff_Duty <= 3 ) {
00138 
00139         msg->data[2] -= ADJUST_DUTY;
00140 
00141     }
00142 }
00143  2021/08/26 R.I.P.
00144 --void Soft_Duty(void) -> void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty) に変更---
00145 void Soft_Duty(void)
00146 {
00147     if(msg1.data[2] == MD1_Duty) {
00148         msg1.data[2] = MD1_Duty;
00149     } else if(msg1.data[2] <= MD1_Duty) {
00150         msg1.data[2] += ADJUST_DUTY;
00151     } else if(msg1.data[2] >= MD1_Duty) {
00152         msg1.data[2] -= ADJUST_DUTY;
00153     }
00154 
00155     if(msg2.data[2] == MD2_Duty) {
00156         msg2.data[2] = MD2_Duty;
00157     } else if(msg2.data[2] <= MD2_Duty) {
00158         msg2.data[2] += ADJUST_DUTY;
00159     } else if(msg2.data[2] >= MD2_Duty) {
00160         msg2.data[2] -= ADJUST_DUTY;
00161     }
00162 }
00163 
00164 
00165 ---[実装未定]PS3コン切替式非常停止---
00166     if( Old_Emg_Flag != Emg_Flag ) {
00167         if( ( Old_Emg_Flag == 0 ) && ( Emg_Flag == 1 ) ) {//立ち上がり
00168             led1 = 0;
00169             emg_stop_tri = 0;//停止
00170         } else if( ( Old_Emg_Flag == 1 ) && ( Emg_Flag == 0 ) ) {//立下り
00171             led1 = 0;
00172             emg_stop_tri = 0;//停止
00173         }
00174     } else {
00175         Old_Emg_Flag = Emg_Flag;
00176     }
00177 
00178 
00179 ---ノードが増えると割込みによる運用が出来なくなる---
00180     ticker.attach(&CAN1_Trans, 0.05);
00181 ---CAN1送信関数---
00182 void CAN1_Trans(void)
00183 {
00184     if( !(can1.write(msg1)) ) {
00185         pc.printf("\033[1;1Hmsg1 failed");
00186     }
00187     if( !(can1.write(msg2)) ) {
00188         pc.printf("\033[2;1Hmsg2 failed");
00189     }
00190     if( !(can1.write(msg3)) ) {
00191         pc.printf("\033[3;1Hmsg3 failed");
00192     }
00193     if( !(can1.write(msg4)) ) {
00194         pc.printf("\033[4;1Hmsg4 failed");
00195     }
00196 }
00197 ***************************************************************************************************/