![](/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:
- 40:2d6888448ab2
- Parent:
- 39:6735743ac0f1
- Child:
- 42:63aedf71f4d1
--- a/main.cpp Thu Sep 07 06:58:57 2017 +0000 +++ b/main.cpp Thu Sep 07 10:12:08 2017 +0000 @@ -33,13 +33,13 @@ #define yaw_Kp 0.01 #define yaw_Ki 0.01 #define yaw_Kd 0.01 -#define acceleration 25 +#define acceleration 20 //25 #define pin_cylinder_on p17 #define pin_cylinder_off p18 #define pin_interrupt_cylinder_min p23 #define encoder_A p25 #define encoder_B p26 -#define enc_Kp 0.007 +#define enc_Kp 0.0400 #define enc_Ki 0.0001 #define enc_Kd 0.0003 #define mecanum_power 1.0 @@ -55,9 +55,9 @@ //#define pin_interrupt_reload p30 // DigitalOut led1(LED1); -//DigitalOut led2(LED2); +DigitalOut led2(LED2); DigitalOut led3(LED3); -//DigitalOut led4(LED4); +DigitalOut led4(LED4); DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); @@ -123,16 +123,20 @@ { setup(); while(1) { + led1 = interrupt_sholderleft_max; + led2 = interrupt_sholderleft_min; + led3 = interrupt_sholderright_max; + led4 = interrupt_sholderright_min; //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg()); - pc.printf("rise state : %d\r\n",cyclic_servo.getState()); + //pc.printf("rise state : %d\r\n",cyclic_servo.getState()); } } void setup() { - wait(0.5); + wait(2.0); bno055.begin(); - wait(0.5); + wait(1.0); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); @@ -155,10 +159,10 @@ //Sword float shoulder_right_cal() { - if(interrupt_sholderright_max==0&&sbdbt.sankaku) { + if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) { return 0.0; } - if(interrupt_sholderright_min==0&&sbdbt.batu) { + if(interrupt_sholderright_min==0&&sbdbt.batu==1) { return 0.0; } return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); @@ -166,13 +170,13 @@ float shoulder_left_cal() { - if(interrupt_sholderleft_max==0&&sbdbt.up==1) { + if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) { return 0.0; } - if(interrupt_sholderleft_min==0&&sbdbt.down==1) { + if(interrupt_sholderleft_min==0&&sbdbt.batu==1) { return 0.0; } - return (sbdbt.sankaku*0.8-sbdbt.batu*0.8); + return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); } void sword_cal() { @@ -183,23 +187,20 @@ void cylinder_origin_first() { while(interrupt_cylinder_min == 1) { - led1 = 1; - rs422.put(5, -0.5, 0.0); + rs422.put(5, -0.4, 0.0); } - led1 = 0; enc_cylinder.origin(); } //cylinder_origin_flagを1で動作する void cylinder_origin() { - if(cylinder_origin_flag == 1){ - rs422.put(5, -0.5, 0.0); - } - if(interrupt_cylinder_min == 0){ + if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1){ cylinder_origin_flag = 0; enc_cylinder.origin(); cylinder_pos_num = 0; + }else if(cylinder_origin_flag == 1){ + rs422.put(5, -0.5, 0.0); } } @@ -302,12 +303,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+sbdbt.right_y), v3.duty(mecanum.v3()*mecanum_power+sbdbt.right_y)); + rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5))); counter++; break; case 1: - //.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)); + //.duty(<cal>*<powerControle>+(<boost>*0.5)) + rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.5)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.5))); counter ++; break; case 2: