ライブラリ化を行った後
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_Practice1 by
main.cpp
00001 //index_include 00002 #include "mbed.h" 00003 #include "math.h" 00004 #include "bit_test.h" 00005 #include "rs422_put.h" 00006 #include "sbdbt.h" 00007 #include "mecanum.h" 00008 #include "bno055_lib.h" 00009 #include "bno055_use.h" 00010 #include "pid.h" 00011 #include "limit.h" 00012 #include "accelerator.h" 00013 #include "pid_encoder.h" 00014 #include "cyclic_var.h" 00015 #include "cyclic_io.h" 00016 #include "cylinder.h" 00017 #include "event_var.h" 00018 00019 //index_define 00020 #define SBDBT_TX p13 00021 #define SBDBT_RX p14 00022 #define SBDBT_STO p12 00023 #define RS422_TX p28 00024 #define RS422_RX p27 00025 #define CONTROL_INDICATOR p11 00026 00027 #define SHOT_CYLINDER_ON p18 00028 #define SHOT_CYLINDER_OFF p17 00029 #define SHOT_INTERRUPT p29 00030 #define SHOT_ENCODER_A p21 00031 #define SHOT_ENCODER_B p22 00032 #define RELOAD_CYLINDER p15 00033 00034 #define SHOULDER_RIGHT_INTERRUPT_MAX p19 00035 #define SHOULDER_RIGHT_INTERRUPT_MIN p20 00036 #define SHOULDER_LEFT_INTERRUPT_MAX p7 00037 #define SHOULDER_LEFT_INTERRUPT_MIN p8 00038 00039 #define nucleo_num 6 00040 #define n1_id 3 00041 #define n2_id 4 00042 #define n3_id 5 00043 #define n4_id 6 00044 #define n5_id 7 00045 #define n6_id 8 00046 00047 //index_namespace_const_variable 00048 namespace BAUD { 00049 const int PC = 460800; 00050 const int SBDBT = 115200; 00051 const int RS422 = 115200; 00052 } 00053 00054 namespace CYCLE { 00055 const float OUTPUT = 0.015; 00056 } 00057 00058 namespace MECANUM { 00059 const float ACCELERATION = 15; 00060 } 00061 00062 namespace YAW { 00063 const float KP = 0.01; 00064 const float KI = 0.01; 00065 const float KD = 0.01; 00066 } 00067 00068 namespace SHOT { 00069 const float ENCORDER_KP = 0.0400; 00070 const float ENCORDER_KI = 0.0001; 00071 const float ENCORDER_KD = 0.0003; 00072 const int CYLINDER_POSITION_MAX = 6; 00073 } 00074 00075 namespace POWER { 00076 const float MECANUM = 1.0; 00077 const float SWORD = 1.0; 00078 const float SHOLDER = 1.0; 00079 } 00080 00081 //index_namespace_variable 00082 namespace yaw{ 00083 float now; 00084 float target; 00085 } 00086 00087 namespace shot{ 00088 int cylinderOriginFlag = 0; 00089 int cylinderPosNum = 0; 00090 float cylinderPos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0}; 00091 float cylinderPwm = 0; 00092 float cylinderPower = 0.0; 00093 } 00094 00095 //index_Function 00096 void setup(); 00097 void output(); 00098 void motorCalculation(); 00099 //void motorBoost(); 00100 float shotCylinderPowerCalclation(); 00101 void shotCylinderCalculation(); 00102 void firstMotion(); 00103 void swordCalculation(); 00104 float shoulderRightCalclation(); 00105 float shoulderLeftCalclation(); 00106 void shoulderInterruptIndicator(); 00107 00108 //index_setupPin 00109 DigitalOut led1(LED1); 00110 DigitalOut led2(LED2); 00111 DigitalOut led3(LED3); 00112 DigitalOut led4(LED4); 00113 Serial pc(USBTX,USBRX); 00114 Rs422 rs422(RS422_TX, RS422_RX); 00115 Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO); 00116 Ticker outputTimer; 00117 Mecanum mecanum; 00118 Bno055 bno055; 00119 PositionPid yawPid; 00120 Accelerator v1; 00121 Accelerator v2; 00122 Accelerator v3; 00123 Accelerator v4; 00124 eventVar shotCylinderEvent; 00125 CyclicVar shotPositionCyclic; 00126 DigitalIn shotInterrupt(SHOT_INTERRUPT); 00127 DigitalOut shotCylinderOn(SHOT_CYLINDER_ON); 00128 DigitalOut shotCylinderOff(SHOT_CYLINDER_OFF); 00129 Encoder shotEncoder(SHOT_ENCODER_A,SHOT_ENCODER_B); 00130 DigitalIn shoulderRightInterruptMin(SHOULDER_RIGHT_INTERRUPT_MIN); 00131 DigitalIn shoulderRightInterruptMax(SHOULDER_RIGHT_INTERRUPT_MAX); 00132 DigitalIn shoulderLeftInterruptMin(SHOULDER_LEFT_INTERRUPT_MIN); 00133 DigitalIn shoulderLeftInterruptMax(SHOULDER_LEFT_INTERRUPT_MAX); 00134 CyclicVar swordSwingCyclic; 00135 CyclicIo reloadCylinder(RELOAD_CYLINDER); 00136 DigitalOut controlIndigator(CONTROL_INDICATOR); 00137 00138 int main() 00139 { 00140 setup(); 00141 while(1) { 00142 //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg()); 00143 //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); 00144 } 00145 } 00146 00147 void setup() 00148 { 00149 wait(1.0); 00150 bno055.begin(); 00151 wait(1.0); 00152 bno055.firstRead(); 00153 pc.baud(BAUD::PC); 00154 sbdbt.begin(BAUD::SBDBT); 00155 rs422.begin(BAUD::RS422); 00156 firstMotion(); 00157 shot::cylinderPosNum = 1; //セットアップタイムでの初期装填のため 00158 outputTimer.attach(&output, CYCLE::OUTPUT); 00159 yawPid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT); 00160 mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180 00161 v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00162 v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00163 v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00164 v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00165 shotEncoder.setPpr(100); 00166 shotEncoder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT); 00167 } 00168 00169 //index_sword 00170 void shoulderInterruptIndicator(){ 00171 led1 = shoulderLeftInterruptMax; 00172 led2 = shoulderLeftInterruptMin; 00173 led3 = shoulderRightInterruptMax; 00174 led4 = shoulderRightInterruptMin; 00175 } 00176 00177 float shoulderRightCalclation(){ 00178 if(shoulderRightInterruptMax==0&&sbdbt.sankaku==1) { 00179 return 0.0; 00180 } 00181 if(shoulderRightInterruptMin==0&&sbdbt.batu==1) { 00182 return 0.0; 00183 } 00184 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); 00185 } 00186 00187 float shoulderLeftCalclation(){ 00188 if(shoulderLeftInterruptMax==0&&sbdbt.sankaku==1) { 00189 return 0.0; 00190 } 00191 if(shoulderLeftInterruptMin==0&&sbdbt.batu==1) { 00192 return 0.0; 00193 } 00194 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); 00195 } 00196 00197 void swordCalculation(){ 00198 swordSwingCyclic.cyclic(sbdbt.maru); 00199 } 00200 00201 //index_shot 00202 void firstMotion(){ 00203 float cylinder = 0.0; 00204 float sholderL = 0.0; 00205 float sholderR = 0.0; 00206 while(shotInterrupt == 1 || shoulderLeftInterruptMax == 1 || shoulderRightInterruptMax == 1){ 00207 if(shotInterrupt == 1){ 00208 cylinder = 2.0; 00209 }else{ 00210 cylinder = 0.0; 00211 } 00212 if(shoulderRightInterruptMax == 1){ 00213 sholderR = -2.0; 00214 }else{ 00215 sholderR = 0.0; 00216 } 00217 if(shoulderLeftInterruptMax == 1){ 00218 sholderL = 2.0; 00219 }else{ 00220 sholderL = 0.0; 00221 } 00222 rs422.put(5, cylinder, 0.0); 00223 rs422.put(6, sholderR, sholderL); 00224 } 00225 shotEncoder.origin(); 00226 } 00227 00228 //cylinderOriginFlagを1にすることで動作する 00229 float shotCylinderPowerCalclation(){ 00230 if(shotInterrupt == 0&&shot::cylinderOriginFlag == 1) { 00231 shot::cylinderOriginFlag = 0; 00232 shotEncoder.origin(); 00233 shot::cylinderPosNum = 0; 00234 shot::cylinderPower = -0.0; 00235 reloadCylinder.cyclicOff(); 00236 return 0.0; 00237 }else if(shot::cylinderOriginFlag == 1){ 00238 return -0.8; 00239 }else{ 00240 return -1*shotEncoder.duty_enableWidth(-10.0,10.0); 00241 } 00242 } 00243 00244 void shotCylinderCalculation(){ 00245 //cylinder ON/OFF 00246 if(sbdbt.shikaku){ 00247 shotCylinderOn = 1; 00248 shotCylinderOff = 0; 00249 }else{ 00250 shotCylinderOn = 0; 00251 shotCylinderOff = 1; 00252 } 00253 shotCylinderEvent.input(sbdbt.right); 00254 if(shotCylinderEvent.getRise()) { //cylinder degset 00255 shot::cylinderPosNum++; 00256 if(shot::cylinderPosNum >= SHOT::CYLINDER_POSITION_MAX) { 00257 //shot::cylinderPosNum = 0; 00258 shot::cylinderOriginFlag=1; 00259 } 00260 } 00261 shotEncoder.cal((float)shot::cylinderPos[shot::cylinderPosNum],CYCLE::OUTPUT); //set cylinder_tergetPos 00262 //pc.printf("target\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",shot::cylinderPos[shot::cylinderPosNum],shotEncoder.deg(),shotEncoder.pulse(),shotEncoder.duty()); 00263 } 00264 00265 //functionBoost 00266 void motorBoost(){ 00267 if(sbdbt.r2) { 00268 mecanum.boost_forward(); 00269 } 00270 if(sbdbt.l2) { 00271 mecanum.boost_back(); 00272 } 00273 /* 00274 if(sbdbt.shikaku) { 00275 mecanum.boost_left(); 00276 } 00277 if(sbdbt.maru) { 00278 mecanum.boost_right(); 00279 } 00280 */ 00281 } 00282 00283 //index_mecanum 00284 void motorCalculation(){ 00285 static float out_right_x; 00286 yaw::now = bno055.getYawRad(); 00287 yaw::target = yaw::now; 00288 yawPid.calculate(yaw::target, yaw::now); 00289 00290 //後進時に方向が逆転するため 00291 if(sbdbt.right_y < 0.0){ 00292 out_right_x = -1.0*sbdbt.right_x; 00293 }else{ 00294 out_right_x = sbdbt.right_x; 00295 } 00296 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad()); 00297 // pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY()); 00298 } 00299 00300 //index_output 00301 void output() 00302 { 00303 motorCalculation(); 00304 shotCylinderCalculation(); 00305 swordCalculation(); 00306 //servo_out(); 00307 //motorBoost(); 00308 controlIndigator = sbdbt.get_pairingState(); 00309 //led1 = sbdbt.get_pairingState(); 00310 shoulderInterruptIndicator(); 00311 if(sbdbt.up)shot::cylinderOriginFlag = 1; 00312 if(sbdbt.left)bno055.yaw_origin(); 00313 reloadCylinder.cyclic(sbdbt.down); 00314 00315 static int counter; 00316 int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id}; 00317 00318 //sbdbtがpairingしている場合のみ動作 00319 if(sbdbt.get_pairingState()) { 00320 switch (counter) { 00321 case 0: 00322 rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0))); 00323 counter++; 00324 break; 00325 case 1: 00326 //.duty(<cal>*<powerControle>+(<motorBoost>*0.5)) 00327 rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0))); 00328 counter ++; 00329 break; 00330 case 2: 00331 //rs422.put(id[counter], sbdbt.right_y, 0.0); 00332 rs422.put(id[counter], limitf(shotCylinderPowerCalclation(),1.0,-1.0), 0.0); 00333 counter ++; 00334 break; 00335 case 3: 00336 rs422.put(id[counter], limitf((-1*shoulderRightCalclation()*POWER::SHOLDER),1.0,-1.0), limitf((shoulderLeftCalclation()*POWER::SHOLDER),1.0,-1.0)); 00337 counter ++; 00338 break; 00339 case 4: 00340 rs422.put(id[counter], limitf(((float)swordSwingCyclic.getState()*POWER::SWORD),1.0,-1.0),0.0); 00341 counter = 0; 00342 break; 00343 default: 00344 break; 00345 } 00346 }else{ 00347 switch (counter) { 00348 case 0: 00349 rs422.put(id[counter],0.0,0.0); 00350 counter++; 00351 break; 00352 case 1: 00353 rs422.put(id[counter],0.0,0.0); 00354 counter ++; 00355 break; 00356 case 2: 00357 rs422.put(id[counter],0.0,0.0); 00358 counter ++; 00359 break; 00360 case 3: 00361 rs422.put(id[counter],0.0,0.0); 00362 counter ++; 00363 break; 00364 case 4: 00365 rs422.put(id[counter],0.0,0.0); 00366 counter = 0; 00367 break; 00368 default: 00369 break; 00370 } 00371 } 00372 }
Generated on Fri Jul 15 2022 04:29:42 by
1.7.2
