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