![](/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:
- 33:64fd1bd83bac
- Parent:
- 32:f535ace7c529
- Child:
- 34:02d605c68bf3
--- a/main.cpp Tue Aug 29 03:01:50 2017 +0000 +++ b/main.cpp Tue Aug 29 06:56:32 2017 +0000 @@ -41,16 +41,18 @@ #define pin_interrupt_cylinder_min p23 #define encoder_A p25 #define encoder_B p26 -#define enc_Kp 0.009 -#define enc_Ki 0.00 -#define enc_Kd 0.00 -#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 +#define enc_Kp 0.005 +#define enc_Ki 0.0001 +#define enc_Kd 0.0003 +#define mecanum_power 0.8 +#define sword_power 0.8 +#define sholder_power 0.8 +//#define pin_servo_reload p29 // +//#define pin_interrupt_reload p30 // +#define pin_interrupt_sholderright_max p21 +#define pin_interrupt_sholderright_min p22 +#define pin_interrupt_sholderleft_max p7 +#define pin_interrupt_sholderleft_min p8 DigitalOut led1(LED1); //DigitalOut led2(LED2); @@ -80,8 +82,8 @@ //追加点 Encoder enc_cylinder(encoder_A,encoder_B); //QEI wheel(encoder_A, encoder_B, NC, 624); -Servo servo_reload(pin_servo_reload); -DigitalIn interrupt_reload(pin_interrupt_reload); +//Servo servo_reload(pin_servo_reload); +//DigitalIn interrupt_reload(pin_interrupt_reload); void setup(); void output(); @@ -145,17 +147,24 @@ cylinder_cal(); sword_cal(); //boost(); + + if(interrupt_cylinder_min==0){ + enc_cylinder.origin(); + } + if(sbdbt.left){ + cylinder_origin(); + } 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()*powerdown), v3.duty(mecanum.v3()*powerdown)); + rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power)); counter++; break; case 1: - rs422.put(id[counter], v2.duty(mecanum.v2()*powerdown), v4.duty(mecanum.v4()*powerdown)); + rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power)); counter ++; break; case 2: @@ -163,11 +172,11 @@ counter ++; break; case 3: - rs422.put(id[counter], shoulder_right_cal(),shoulder_right_cal()); + rs422.put(id[counter], shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power); counter ++; break; case 4: - rs422.put(id[counter], ((float)sword.getState()*0.8),0.0); + rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); counter = 0; break; default: @@ -176,20 +185,20 @@ } float shoulder_right_cal(){ - if(interrupt_sholderright_max==1&&sbdbt.batu==1){ + if(interrupt_sholderright_max==0&&sbdbt.sankaku){ return 0.0; } - if(interrupt_sholderright_min==1&&sbdbt.sankaku==1){ - return 0.0; + if(interrupt_sholderright_min==0&&sbdbt.batu){ + return 0.0; } return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); } float shoulder_left_cal(){ - if(interrupt_sholderright_max==1&&sbdbt.up==1){ + if(interrupt_sholderleft_max==0&&sbdbt.up==1){ return 0.0; } - if(interrupt_sholderright_min==1&&sbdbt.down==1){ + if(interrupt_sholderleft_min==0&&sbdbt.down==1){ return 0.0; } return (sbdbt.up*0.8-sbdbt.down*0.8); @@ -226,7 +235,7 @@ } enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定 - pc.printf("terget\t%f\tnow_deg\t%f\tnow_rad\t%f\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.rad(),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()); //リロード機構完成後 /*