sampleProgram
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_fuzi by
Diff: main.cpp
- Revision:
- 35:5e1ad00f26fb
- Parent:
- 34:02d605c68bf3
- Child:
- 36:dca1081c19b3
--- a/main.cpp Tue Aug 29 11:30:28 2017 +0000 +++ b/main.cpp Wed Sep 06 01:17:44 2017 +0000 @@ -13,8 +13,6 @@ #include "cyclic.h" #include "cyclic_IO.h" #include "cylinder.h" -#include "Servo.h" -//#include "QEI.h" #define pc_baud 460800 #define sbdbt_tx p13 @@ -47,12 +45,14 @@ #define mecanum_power 1.0 #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 +#define pin_servo p29 +#define servo_reload_time 1.0 +//#define pin_servo_reload p29 // +//#define pin_interrupt_reload p30 // DigitalOut led1(LED1); //DigitalOut led2(LED2); @@ -77,11 +77,11 @@ 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); -//追加点 -Encoder enc_cylinder(encoder_A,encoder_B); -//QEI wheel(encoder_A, encoder_B, NC, 624); +//追加 +PwmOut servo(pin_servo); +Timeout timer_servo; //Servo servo_reload(pin_servo_reload); //DigitalIn interrupt_reload(pin_interrupt_reload); @@ -92,6 +92,8 @@ void boost(); void cylinder_origin(); void sword_cal(); +void cylinder_reload(); +void servo_origin(); float shoulder_right_cal(); float shoulder_left_cal(); int reload_magazine_flag; @@ -120,9 +122,9 @@ void setup() { - wait(1); + wait(0.5); bno055.begin(); - wait(1); + wait(0.5); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); @@ -135,73 +137,34 @@ v2.setup(acceleration,output_period); v3.setup(acceleration,output_period); v4.setup(acceleration,output_period); - -//追加点 enc_cylinder.setup(100); enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); + + //追加 + servo.period(0.020); } -void output() + +float shoulder_right_cal() { - motor_cal(); - 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()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power)); - counter++; - break; - case 1: - rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power)); - 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_right_cal()*sholder_power); - counter ++; - break; - case 4: - rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); - counter = 0; - break; - default: - break; - }; -} - -float shoulder_right_cal(){ - if(interrupt_sholderright_max==0&&sbdbt.sankaku){ + if(interrupt_sholderright_max==0&&sbdbt.sankaku) { return 0.0; } - if(interrupt_sholderright_min==0&&sbdbt.batu){ + 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_sholderleft_max==0&&sbdbt.up==1){ +float shoulder_left_cal() +{ + if(interrupt_sholderleft_max==0&&sbdbt.up==1) { return 0.0; } - if(interrupt_sholderleft_min==0&&sbdbt.down==1){ - return 0.0; + if(interrupt_sholderleft_min==0&&sbdbt.down==1) { + return 0.0; } - return (sbdbt.up*0.8-sbdbt.down*0.8); + return (sbdbt.up*0.8-sbdbt.down*0.8); } void sword_cal() @@ -219,6 +182,7 @@ enc_cylinder.origin(); } + //追記(動作未確認) float cylinder_pwm; int cylinder_pos_num = 0; @@ -226,15 +190,15 @@ void cylinder_cal() { cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF - - if(riseEvent(sbdbt.right)){ //cylinder degset + + if(riseEvent(sbdbt.right)) { //cylinder degset cylinder_pos_num++; - if(cylinder_pos_num >= 3){ + 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_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()); //リロード機構完成後 @@ -253,6 +217,27 @@ */ } +//追記(動作未確認) +//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_origin() +{ + servo.pulsewidth(0.0010); + timer_servo.detach(); +} + void boost() { if(sbdbt.r2) { @@ -278,4 +263,55 @@ yaw_pid.cal(target_yaw, yaw, output_period); mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad()); // pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY()); +} + + +void output() +{ + motor_cal(); + cylinder_cal(); + sword_cal(); + //boost(); + + //インタラプタが反応したとき原点を取る + /* + if(interrupt_cylinder_min==0) { + enc_cylinder.origin(); + } + */ + + 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}; + switch (counter) { + case 0: + 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()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power)); + 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_right_cal()*sholder_power); + counter ++; + break; + case 4: + rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0); + counter = 0; + break; + default: + break; + }; } \ No newline at end of file