![](/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:
- 31:285c9898da03
- Parent:
- 30:57061e222f10
- Child:
- 32:f535ace7c529
--- a/main.cpp Thu Aug 24 18:20:12 2017 +0000 +++ b/main.cpp Tue Aug 29 02:07:56 2017 +0000 @@ -38,7 +38,7 @@ #define acceleration 25 #define pin_cylinder_on p17 #define pin_cylinder_off p18 -#define pin_interrupt p23 +#define pin_interrupt_cylinder_min p23 #define encoder_A p25 #define encoder_B p26 #define enc_Kp 0.01 @@ -47,12 +47,16 @@ #define powerdown 0.6 #define pin_servo_reload p21 // #define pin_interrupt_reload p22 // +#define pin_interrupt_sholderright_max p23 +#define pin_interrupt_sholderright_min p24 +#define pin_interrupt_sholderleft_max p29 +#define pin_interrupt_sholderleft_min p30 DigitalOut led1(LED1); //DigitalOut led2(LED2); DigitalOut led3(LED3); //DigitalOut led4(LED4); -DigitalIn interrupt(pin_interrupt); +DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); Sbdbt sbdbt(sbdbt_tx, sbdbt_rx); @@ -66,6 +70,13 @@ Accel v4; Cylinder cylinder(pin_cylinder_on,pin_cylinder_off); Cyclic sword; +Cyclic cyclic_cylinder_position; +DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min); +DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max); +DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min); +DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max); + + //追加点 Encoder enc_cylinder(encoder_A,encoder_B); //QEI wheel(encoder_A, encoder_B, NC, 624); @@ -79,14 +90,29 @@ void boost(); void cylinder_origin(); void sword_cal(); +float shoulder_right_cal(); +float shoulder_left_cal(); int reload_magazine_flag; float yaw, target_yaw; +//riseEventそのうちClassにしたい +short state; +int riseEvent(int input) +{ + state = ((state<<1)|input)&3; + if(state == 1) { + return 1; + } else { + return 0; + } +} + int main() { setup(); while(1) { - pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg()); + //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)); } } @@ -109,7 +135,7 @@ v4.setup(acceleration,output_period); //追加点 - enc_cylinder.setup(1200); + enc_cylinder.setup(100); enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); } @@ -137,7 +163,7 @@ counter ++; break; case 3: - rs422.put(id[counter], (-sbdbt.sankaku*0.8+sbdbt.batu*0.8), (sbdbt.up*0.8-sbdbt.down*0.8)); + rs422.put(id[counter], shoulder_right_cal(),shoulder_right_cal()); counter ++; break; case 4: @@ -149,42 +175,81 @@ }; } -void sword_cal(){ +float shoulder_right_cal(){ + if(interrupt_sholderright_max==1&&sbdbt.batu==1){ + return 0.0; + } + if(interrupt_sholderright_min==1&&sbdbt.sankaku==1){ + return 0.0; + } + return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); +} + +float shoulder_left_cal(){ + if(interrupt_sholderright_max==1&&sbdbt.up==1){ + return 0.0; + } + if(interrupt_sholderright_min==1&&sbdbt.down==1){ + return 0.0; + } + return (sbdbt.up*0.8-sbdbt.down*0.8); +} + +void sword_cal() +{ sword.cyclic(sbdbt.maru); } -void cylinder_origin(){ - while(interrupt){ +void cylinder_origin() +{ + while(interrupt_cylinder_min == 1) { led1 = 1; - rs422.put(5, -0.8, 0.0); + rs422.put(5, -0.2, 0.0); } led1 = 0; + enc_cylinder.origin(); } //追記(動作未確認) +float cylinder_pwm; +int cylinder_pos_num = 0; +float cylinder_pos[3] = {0.0,50.0,75.0}; void cylinder_cal() -{ - cylinder.cyclic(sbdbt.shikaku); - enc_cylinder.deg_cylinder_cal(); +{ + cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF + + if(riseEvent(sbdbt.right)){ //cylinder degset + cylinder_pos_num++; + if(cylinder_pos_num >= 3){ + cylinder_pos_num = 0; + } + } + enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定 + + pc.printf("terget\t%f\tnow_pulse\t%lo\tnow_rad\t%f\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.pulse(),enc_cylinder.deg(),enc_cylinder.duty()); - if(cylinder.getInState() == 1){ - if(interrupt)servo_reload = 1.0; - }else{ - servo_reload = 0.0; - reload_magazine_flag = 1; - } - - if(reload_magazine_flag == 1){ - rs422.put(n6_id, 0.8, 0.0); - if(interrupt_reload == 1)reload_magazine_flag = 0; - } +//リロード機構完成後 + /* + if(cylinder.getInState() == 1){ + if(interrupt_cylinder_min)servo_reload = 1.0; + }else{ + servo_reload = 0.0; + reload_magazine_flag = 1; + } + + if(reload_magazine_flag == 1){ + rs422.put(n6_id, 0.8, 0.0); + if(interrupt_reload == 1)reload_magazine_flag = 0; + } + */ } -void boost(){ - if(sbdbt.r2){ +void boost() +{ + if(sbdbt.r2) { mecanum.boost_forward(); } - if(sbdbt.l2){ + if(sbdbt.l2) { mecanum.boost_back(); } /*