![](/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:
- 39:6735743ac0f1
- Parent:
- 38:b071512af5ca
- Child:
- 40:2d6888448ab2
--- a/main.cpp Thu Sep 07 02:21:24 2017 +0000 +++ b/main.cpp Thu Sep 07 06:58:57 2017 +0000 @@ -39,7 +39,7 @@ #define pin_interrupt_cylinder_min p23 #define encoder_A p25 #define encoder_B p26 -#define enc_Kp 0.005 +#define enc_Kp 0.007 #define enc_Ki 0.0001 #define enc_Kd 0.0003 #define mecanum_power 1.0 @@ -98,9 +98,14 @@ void servo_origin(); float shoulder_right_cal(); float shoulder_left_cal(); -int reload_magazine_flag; +//output float yaw, target_yaw; +//cylinder_origin int cylinder_origin_flag = 0; +//cylinder +float cylinder_pwm; +int cylinder_pos_num = 0; +float cylinder_pos[3] = {0.0,180.0,360.0}; //riseEventそのうちClassにしたい short state; @@ -119,7 +124,7 @@ setup(); while(1) { //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg()); - //pc.printf("rise state : %d\r\n",riseEvent(sbdbt.right)); + pc.printf("rise state : %d\r\n",cyclic_servo.getState()); } } @@ -135,7 +140,7 @@ cylinder_origin_first(); output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); - mecanum.setupdeg(bno055.getYawRad()+180.0); + mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180 v1.setup(acceleration,output_period); v2.setup(acceleration,output_period); v3.setup(acceleration,output_period); @@ -179,7 +184,7 @@ { while(interrupt_cylinder_min == 1) { led1 = 1; - rs422.put(5, -0.2, 0.0); + rs422.put(5, -0.5, 0.0); } led1 = 0; enc_cylinder.origin(); @@ -189,18 +194,16 @@ void cylinder_origin() { if(cylinder_origin_flag == 1){ - rs422.put(5, -0.2, 0.0); + rs422.put(5, -0.5, 0.0); } if(interrupt_cylinder_min == 0){ - cylinder_origin_flag = 0; - enc_cylinder.origin(); + cylinder_origin_flag = 0; + enc_cylinder.origin(); + cylinder_pos_num = 0; } } //追記(動作未確認) -float cylinder_pwm; -int cylinder_pos_num = 0; -float cylinder_pos[3] = {0.0,180.0,360.0}; void cylinder_cal() { cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF @@ -234,20 +237,16 @@ void servo_max(){ servo.pulsewidth(0.0022); } - void servo_min() { servo.pulsewidth(0.0010); } - void servo_out(){ - cyclic_servo.cyclic(sbdbt.down); - switch (cyclic_servo.getState()){ - case 0: - servo_min(); - case 1: - servo_max(); - default: + cyclic_servo.cyclic(sbdbt.down); //setServoControl + if(cyclic_servo.getState()==0){ + servo_min(); + }else if(cyclic_servo.getState()==1){ + servo_max(); } } @@ -303,11 +302,12 @@ int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id}; switch (counter) { case 0: - rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power)); + rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power+sbdbt.right_y), v3.duty(mecanum.v3()*mecanum_power+sbdbt.right_y)); counter++; break; case 1: - rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power)); + //.duty(<cal>*<powerControle>+<boost>) + rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power-sbdbt.right_y), v4.duty(mecanum.v4()*mecanum_power-sbdbt.right_y)); counter ++; break; case 2: