![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライブラリ化を行った後
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
Diff: main.cpp
- Revision:
- 53:701dc7886fff
- Parent:
- 52:f5ae47e683fa
- Child:
- 54:a88208d9bc1d
diff -r f5ae47e683fa -r 701dc7886fff main.cpp --- a/main.cpp Thu Sep 14 11:56:51 2017 +0000 +++ b/main.cpp Fri Sep 15 04:12:06 2017 +0000 @@ -93,7 +93,7 @@ void cylinder_cal(); void boost(); void cylinder_origin(); -void cylinder_origin_first(); +void firstmotion(); void sword_cal(); void servo_origin(); float shoulder_right_cal(); @@ -131,7 +131,7 @@ pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); -// cylinder_origin_first(); + firstmotion(); cylinder_pos_num = 2; //セットアップタイムでの初期装填のため output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); @@ -173,12 +173,30 @@ } //cylinder -void cylinder_origin_first() +void firstmotion() { - while(interrupt_cylinder_min == 1){ - rs422.put(5, -0.8, 0.0); + while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){ + float cylinder = 0.0; + float sholderL = 0.0; + float sholderR = 0.0; + if(interrupt_cylinder_min == 1){ + cylinder = 2.0; + }else{ + cylinder = 0.0; + } + if(interrupt_sholderright_max == 1){ + sholderR = -2.0; + }else{ + sholderR = 0.0; + } + if(interrupt_sholderleft_max == 1){ + sholderL = 2.0; + }else{ + sholderL = 0.0; + } + rs422.put(5, cylinder, 0.0); + rs422.put(6, sholderR, sholderL); } - rs422.put(5, 0.0, 0.0); enc_cylinder.origin(); } @@ -275,30 +293,31 @@ cylinder_reload.cyclic(sbdbt.down); static int counter; + static float mecanumV1, mecanumV2, mecanumV3, mecanumV4; int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id}; //sbdbtがpairingしている場合のみ動作 if(sbdbt.get_pairingState()) { switch (counter) { case 0: - rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8))); + rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0))); counter++; break; case 1: //.duty(<cal>*<powerControle>+(<boost>*0.5)) - rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8))); + rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0))); counter ++; break; case 2: - rs422.put(id[counter], -1*enc_cylinder.duty_enableWidth(-5.0,5.0), 0.0); + rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-5.0,5.0)),1.0,-1.0), 0.0); counter ++; break; case 3: - rs422.put(id[counter], -1*shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power); + rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0)); counter ++; break; case 4: - rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); + rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0); counter = 0; break; default: