![](/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:
- 71:7d684ff43ddd
- Parent:
- 69:b2c8a0c5662c
- Child:
- 72:7d6177047823
--- a/main.cpp Sun Sep 24 08:37:15 2017 +0000 +++ b/main.cpp Tue Sep 26 08:39:55 2017 +0000 @@ -15,51 +15,69 @@ #include "cyclic_io.h" #include "cylinder.h" #include "event_var.h" -#include "servo.h" +//#include "servo.h" //index_define -#define pc_baud 460800 -#define sbdbt_tx p13 -#define sbdbt_rx p14 -#define sbdbt_baud 115200 -#define rs422_tx p28 -#define rs422_rx p27 -#define rs422_baud 115200 -#define output_period 0.015 +#define SBDBT_TX p13 +#define SBDBT_RX p14 +#define SBDBT_STO p12 +#define RS422_TX p28 +#define RS422_RX p27 +#define CONTROL_INDICATOR p11 + +namespace BAUD { + const int PC = 460800; + const int SBDBT = 115200; + const int RS422 = 115200; +} + +namespace CYCLE { + const float OUTPUT = 0.015; +} + +namespace MECANUM { + const float ACCELERATION = 15; +} + +namespace YAW { + const float KP = 0.01; + const float KI = 0.01; + const float KD = 0.01; +} + +namespace SHOT { + const float ENCORDER_KP = 0.0400; + const float ENCORDER_KI = 0.0001; + const float ENCORDER_KD = 0.0003; + const int CYLINDER_POSITION_MAX = 6; +} + + #define nucleo_num 6 -#define pi 3.141592 #define n1_id 3 #define n2_id 4 #define n3_id 5 #define n4_id 6 #define n5_id 7 #define n6_id 8 -#define yaw_Kp 0.01 -#define yaw_Ki 0.01 -#define yaw_Kd 0.01 -#define acceleration 15 //25 -#define pin_cylinder_on p18 -#define pin_cylinder_off p17 -#define pin_interrupt_cylinder_min p29 -#define encoder_A p21 -#define encoder_B p22 -#define enc_Kp 0.0400 -#define enc_Ki 0.0001 -#define enc_Kd 0.0003 + +#define SHOT_CYLINDER_ON p18 +#define SHOT_CYLINDER_OFF p17 +#define SHOT_INTERRUPT p29 +#define SHOT_ENCODER_A p21 +#define SHOT_ENCODER_B p22 +#define RELOAD_CYLINDER p15 + #define mecanum_power 1.0 #define sword_power 1.0 #define sholder_power 1.0 -#define pin_interrupt_sholderright_max p19 //p21 -#define pin_interrupt_sholderright_min p20 //p22 -#define pin_interrupt_sholderleft_max p7 -#define pin_interrupt_sholderleft_min p8 +#define SHOULDER_RIGHT_INTERRUPT_MAX p19 //p21 +#define SHOULDER_RIGHT_INTERRUPT_MIN p20 //p22 +#define SHOULDER_LEFT_INTERRUPT_MAX p7 +#define SHOULDER_LEFT_INTERRUPT_MIN p8 //#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 cylinder_pos_max 6 +//#define servo_reload_time 1.0 //index_setupPin @@ -67,10 +85,10 @@ DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); -DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); +DigitalIn interrupt_cylinder_min(SHOT_INTERRUPT); Serial pc(USBTX,USBRX); -Rs422 rs422(rs422_tx, rs422_rx); -Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing); +Rs422 rs422(RS422_TX, RS422_RX); +Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO); Ticker output_timer; Mecanum mecanum; Bno055 bno055; @@ -79,19 +97,19 @@ Accelerator v2; Accelerator v3; Accelerator v4; -DigitalOut cylinder_on(pin_cylinder_on); -DigitalOut cylinder_off(pin_cylinder_off); +DigitalOut cylinder_on(SHOT_CYLINDER_ON); +DigitalOut cylinder_off(SHOT_CYLINDER_OFF); CyclicVar sword; CyclicVar 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); +DigitalIn interrupt_sholderright_min(SHOULDER_RIGHT_INTERRUPT_MIN); +DigitalIn interrupt_sholderright_max(SHOULDER_RIGHT_INTERRUPT_MAX); +DigitalIn interrupt_sholderleft_min(SHOULDER_LEFT_INTERRUPT_MIN); +DigitalIn interrupt_sholderleft_max(SHOULDER_LEFT_INTERRUPT_MAX); +Encoder enc_cylinder(SHOT_ENCODER_A,SHOT_ENCODER_B); CyclicVar cyclic_servo; -CyclicIo cylinder_reload(pin_cylinder_reload); +CyclicIo cylinder_reload(RELOAD_CYLINDER); //PwmOut servo(pin_servo); -DigitalOut sbdbt_indigator(pin_sbdbt_indicator); +DigitalOut sbdbt_indigator(CONTROL_INDICATOR); eventVar cylinder_event; @@ -116,7 +134,8 @@ //cylinder float cylinder_pwm; int cylinder_pos_num = 0; -float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,450.0,580.0,700.0}; +float cylinder_pos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0}; +float cylinder_power = 0.0; int main() { @@ -138,20 +157,20 @@ bno055.begin(); wait(1.0); bno055.firstRead(); - pc.baud(pc_baud); - sbdbt.begin(sbdbt_baud); - rs422.begin(rs422_baud); + pc.baud(BAUD::PC); + sbdbt.begin(BAUD::SBDBT); + rs422.begin(BAUD::RS422); firstmotion(); cylinder_pos_num = 2; //セットアップタイムでの初期装填のため - output_timer.attach(&output, output_period); - yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd,output_period); + output_timer.attach(&output, CYCLE::OUTPUT); + yaw_pid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT); mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180 - v1.setup(acceleration,output_period); - v2.setup(acceleration,output_period); - v3.setup(acceleration,output_period); - v4.setup(acceleration,output_period); + v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); + v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); + v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); + v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); enc_cylinder.setPpr(100); - enc_cylinder.setup(enc_Kp,enc_Ki,enc_Kd,output_period); + enc_cylinder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT); //servo.period(0.020); } @@ -213,14 +232,20 @@ //cylinder_origin_flagを1にすることで動作する void cylinder_origin() { - 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) { - rs422.put(5, -0.8, 0.0); - } } +float cylinder_power_cal(){ + if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) { + cylinder_origin_flag = 0; + enc_cylinder.origin(); + cylinder_pos_num = 0; + cylinder_power = -0.0; + }else if(cylinder_origin_flag == 1){ + return -0.8; + }else{ + return -1*enc_cylinder.duty_enableWidth(-10.0,10.0); + } +} + void cylinder_cal() { //cylinder ON/OFF @@ -235,12 +260,12 @@ cylinder_event.input(sbdbt.right); if(cylinder_event.getRise()) { //cylinder degset cylinder_pos_num++; - if(cylinder_pos_num >= cylinder_pos_max) { + if(cylinder_pos_num >= SHOT::CYLINDER_POSITION_MAX) { //cylinder_pos_num = 0; cylinder_origin_flag=1; } } - enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //set cylinder_tergetPos + enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],CYCLE::OUTPUT); //set cylinder_tergetPos 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()); } @@ -333,7 +358,7 @@ break; case 2: //rs422.put(id[counter], sbdbt.right_y, 0.0); - rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-10.0,10.0)),1.0,-1.0), 0.0); + rs422.put(id[counter], limitf(cylinder_power_cal(),1.0,-1.0), 0.0); counter ++; break; case 3: