![](/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:
- 38:b071512af5ca
- Parent:
- 37:fa738e34c4d3
- Child:
- 39:6735743ac0f1
diff -r fa738e34c4d3 -r b071512af5ca main.cpp --- a/main.cpp Wed Sep 06 02:57:38 2017 +0000 +++ b/main.cpp Thu Sep 07 02:21:24 2017 +0000 @@ -78,6 +78,7 @@ DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min); DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max); Encoder enc_cylinder(encoder_A,encoder_B); +Cyclic cyclic_servo; //追加 PwmOut servo(pin_servo); @@ -91,6 +92,7 @@ void cylinder_cal(); void boost(); void cylinder_origin(); +void cylinder_origin_first(); void sword_cal(); void cylinder_reload(); void servo_origin(); @@ -98,6 +100,7 @@ float shoulder_left_cal(); int reload_magazine_flag; float yaw, target_yaw; +int cylinder_origin_flag = 0; //riseEventそのうちClassにしたい short state; @@ -129,7 +132,7 @@ pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); - cylinder_origin(); + cylinder_origin_first(); output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); mecanum.setupdeg(bno055.getYawRad()+180.0); @@ -172,7 +175,7 @@ } //cylinder -void cylinder_origin() +void cylinder_origin_first() { while(interrupt_cylinder_min == 1) { led1 = 1; @@ -181,6 +184,19 @@ led1 = 0; enc_cylinder.origin(); } + +//cylinder_origin_flagを1で動作する +void cylinder_origin() +{ + if(cylinder_origin_flag == 1){ + rs422.put(5, -0.2, 0.0); + } + if(interrupt_cylinder_min == 0){ + cylinder_origin_flag = 0; + enc_cylinder.origin(); + } +} + //追記(動作未確認) float cylinder_pwm; int cylinder_pos_num = 0; @@ -197,7 +213,7 @@ } enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定 - pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty()); + //pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty()); //リロード機構完成後 /* @@ -214,24 +230,25 @@ } */ } -//追記(動作未確認) -//SBDBTか何かでcylinder_reload_enableを1にすりゃ動く -int cylinder_reload_enable = 0; -void cylinder_reload() -{ - if(cylinder_reload_enable == 1) { - cylinder_pos_num = 0; - if(interrupt_cylinder_min == 0) { - servo.pulsewidth(0.0022); - cylinder_reload_enable = 0; - timer_servo.attach(&servo_origin, servo_reload_time); - } - } + +void servo_max(){ + servo.pulsewidth(0.0022); } -void servo_origin() + +void servo_min() { servo.pulsewidth(0.0010); - timer_servo.detach(); +} + +void servo_out(){ + cyclic_servo.cyclic(sbdbt.down); + switch (cyclic_servo.getState()){ + case 0: + servo_min(); + case 1: + servo_max(); + default: + } } @@ -268,22 +285,19 @@ motor_cal(); cylinder_cal(); sword_cal(); + servo_out(); + cylinder_origin(); //boost(); //インタラプタが反応したとき原点を取る - /* - if(interrupt_cylinder_min==0) { - enc_cylinder.origin(); + if(sbdbt.up) { + cylinder_origin_flag = 1; } - */ if(sbdbt.left) { cylinder_origin(); } - if(sbdbt.down){ - cylinder_reload_enable = 1; - } static int counter; int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};