![](/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:
- 45:a32e8091901b
- Parent:
- 44:7410d8356f58
- Child:
- 47:6ea046767494
--- a/main.cpp Sun Sep 10 11:51:57 2017 +0000 +++ b/main.cpp Tue Sep 12 08:29:39 2017 +0000 @@ -53,17 +53,19 @@ #define pin_servo p21 #define servo_reload_time 1.0 #define pin_cylinder_reload p15 +#define pin_sbdbt_pairing p12 +#define pin_sbdbt_indicator p11 //#define pin_servo_reload p29 // //#define pin_interrupt_reload p30 // -//DigitalOut led1(LED1); +DigitalOut led1(LED1); //DigitalOut led2(LED2); //DigitalOut led3(LED3); //DigitalOut led4(LED4); DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); -Sbdbt sbdbt(sbdbt_tx, sbdbt_rx); +Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing); Ticker output_timer; Mecanum mecanum; Bno055 bno055; @@ -86,6 +88,7 @@ //追加 PwmOut servo(pin_servo); Timeout timer_servo; +DigitalOut sbdbt_indigator(pin_sbdbt_indicator); //Servo servo_reload(pin_servo_reload); //DigitalIn interrupt_reload(pin_interrupt_reload); @@ -132,6 +135,7 @@ // 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("Sbdbtstate : %d\r\n",sbdbt.get_pairingState()); } } @@ -199,11 +203,11 @@ //cylinder_origin_flagを1にすることで動作する void cylinder_origin() { - if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1){ + 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){ + } else if(cylinder_origin_flag == 1) { rs422.put(5, -0.8, 0.0); } } @@ -240,18 +244,20 @@ */ } -void servo_max(){ - servo.pulsewidth(0.0022); +void servo_max() +{ + servo.pulsewidth(0.0022); } void servo_min() { servo.pulsewidth(0.0010); } -void servo_out(){ +void servo_out() +{ cyclic_servo.cyclic(sbdbt.down); //setServoControl - if(cyclic_servo.getState()==0){ + if(cyclic_servo.getState()==0) { servo_min(); - }else if(cyclic_servo.getState()==1){ + } else if(cyclic_servo.getState()==1) { servo_max(); } } @@ -293,39 +299,49 @@ servo_out(); cylinder_origin(); //boost(); - - //インタラプタが反応したとき原点を取る + + led1 = sbdbt.get_pairingState(); + sbdbt_indigator = sbdbt.get_pairingState(); + if(sbdbt.up) { cylinder_origin_flag = 1; } - + cylinder_reload.cyclic(sbdbt.left); - + static int counter; 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*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5))); - counter++; - break; - case 1: - //.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: - rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0); - counter ++; - break; - case 3: - rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power); - counter ++; - break; - case 4: - rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); - counter = 0; - break; - default: - break; - }; + + //sbdbtがpairingしている場合のみ動作 + if(sbdbt.get_pairingState()) { + switch (counter) { + case 0: + 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>*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: + rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0); + counter ++; + break; + case 3: + rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power); + counter ++; + break; + case 4: + rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); + counter = 0; + break; + default: + break; + }; + }else{ + for(int count_i = 0;count_i<=nucleo_num;count_i++){ + rs422.put(id[count_i],0.0,0.0); + } + } } \ No newline at end of file