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
main.cpp
- Committer:
- echo_piyo
- Date:
- 2017-09-19
- Revision:
- 59:68a73b9d27f5
- Parent:
- 58:6cd46488f59b
- Child:
- 60:07d7bde726f9
- Child:
- 63:7e7d0cbc06c3
- Child:
- 64:41dcec6c20bc
File content as of revision 59:68a73b9d27f5:
//index_include #include "mbed.h" #include "math.h" #include "bit_test.h" #include "RS422_put.h" #include "sbdbt.h" #include "mecanum.h" #include "bno055_lib.h" #include "bno055_use.h" #include "pid.h" #include "limit.h" #include "accelerator.h" #include "encorder.h" #include "cyclic.h" #include "cyclic_IO.h" #include "cylinder.h" #include "varEvent.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 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 p23 #define encoder_A p25 #define encoder_B p26 #define enc_Kp 0.0400 #define enc_Ki 0.0001 #define enc_Kd 0.0003 #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 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 //index_setupPin DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing); Ticker output_timer; Mecanum mecanum; Bno055 bno055; Position_pid yaw_pid; Accel v1; Accel v2; Accel v3; Accel v4; DigitalOut cylinder_on(pin_cylinder_on); DigitalOut cylinder_off(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); Cyclic cyclic_servo; Cyclic_IO cylinder_reload(pin_cylinder_reload); PwmOut servo(pin_servo); DigitalOut sbdbt_indigator(pin_sbdbt_indicator); varEvent cylinder_event; //index_setupFunction void setup(); void output(); void motor_cal(); void cylinder_cal(); void boost(); void cylinder_origin(); void firstmotion(); void sword_cal(); void servo_origin(); float shoulder_right_cal(); float shoulder_left_cal(); //index_setupVariable //output float yaw, target_yaw; //cylinder_origin int cylinder_origin_flag = 0; //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}; int main() { setup(); while(1) { led1 = interrupt_sholderleft_max; led2 = interrupt_sholderleft_min; led3 = interrupt_sholderright_max; led4 = interrupt_sholderright_min; pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg()); //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); } } void setup() { wait(2.0); bno055.begin(); wait(1.0); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); firstmotion(); cylinder_pos_num = 2; //セットアップタイムでの初期装填のため output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); 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); enc_cylinder.setup(100); enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); servo.period(0.020); } //Sword float shoulder_right_cal() { if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) { return 0.0; } if(interrupt_sholderright_min==0&&sbdbt.batu==1) { return 0.0; } return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); } float shoulder_left_cal() { if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) { return 0.0; } if(interrupt_sholderleft_min==0&&sbdbt.batu==1) { return 0.0; } return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8); } void sword_cal() { sword.cyclic(sbdbt.maru); } //cylinder void firstmotion() { while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){ float cylinder = 0.0; float sholderL = 0.0; float sholderR = 0.0; if(interrupt_cylinder_min == 1){ cylinder = 2.0; }else{ cylinder = 0.0; } if(interrupt_sholderright_max == 1){ sholderR = -2.0; }else{ sholderR = 0.0; } if(interrupt_sholderleft_max == 1){ sholderL = 2.0; }else{ sholderL = 0.0; } rs422.put(5, cylinder, 0.0); rs422.put(6, sholderR, sholderL); } enc_cylinder.origin(); } //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); } } void cylinder_cal() { //cylinder ON/OFF if(sbdbt.shikaku){ cylinder_on = 1; cylinder_off = 0; }else{ cylinder_on = 0; cylinder_off = 1; } cylinder_event.input(sbdbt.right); if(cylinder_event.getRise()) { //cylinder degset cylinder_pos_num++; if(cylinder_pos_num >= cylinder_pos_max) { //cylinder_pos_num = 0; cylinder_origin_flag=1; } } enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //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()); } void servo_max() { servo.pulsewidth(0.0022); } void servo_min() { servo.pulsewidth(0.0010); } void servo_out() { cyclic_servo.cyclic(sbdbt.down); //setServoControl if(cyclic_servo.getState()==0) { servo_min(); } else if(cyclic_servo.getState()==1) { servo_max(); } } //boost void boost() { if(sbdbt.r2) { mecanum.boost_forward(); } if(sbdbt.l2) { mecanum.boost_back(); } /* if(sbdbt.shikaku) { mecanum.boost_left(); } if(sbdbt.maru) { mecanum.boost_right(); } */ } //mecanum void motor_cal() { static float out_right_x; yaw = bno055.getYawRad(); target_yaw = yaw; yaw_pid.cal(target_yaw, yaw, output_period); //後進時に方向が逆転するため if(sbdbt.right_y < 0.0){ out_right_x = -1.0*sbdbt.right_x; }else{ out_right_x = sbdbt.right_x; } mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, 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(); servo_out(); cylinder_origin(); //boost(); sbdbt_indigator = sbdbt.get_pairingState(); //led1 = sbdbt.get_pairingState(); if(sbdbt.up)cylinder_origin_flag = 1; if(sbdbt.left)bno055.yaw_origin(); cylinder_reload.cyclic(sbdbt.down); static int counter; static float mecanumV1, mecanumV2, mecanumV3, mecanumV4; int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id}; //sbdbtがpairingしている場合のみ動作 if(sbdbt.get_pairingState()) { switch (counter) { case 0: rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0))); counter++; break; case 1: //.duty(<cal>*<powerControle>+(<boost>*0.5)) rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0))); counter ++; break; case 2: //rs422.put(id[counter], sbdbt.right_y, 0.0); rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-5.0,5.0)),1.0,-1.0), 0.0); counter ++; break; case 3: rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0)); counter ++; break; case 4: rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0); counter = 0; break; default: break; } }else{ switch (counter) { case 0: rs422.put(id[counter],0.0,0.0); counter++; break; case 1: rs422.put(id[counter],0.0,0.0); counter ++; break; case 2: rs422.put(id[counter],0.0,0.0); counter ++; break; case 3: rs422.put(id[counter],0.0,0.0); counter ++; break; case 4: rs422.put(id[counter],0.0,0.0); counter = 0; break; default: break; } } }