![](/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
main.cpp
- Committer:
- echo_piyo
- Date:
- 2017-08-29
- Revision:
- 32:f535ace7c529
- Parent:
- 31:285c9898da03
- Child:
- 33:64fd1bd83bac
File content as of revision 32:f535ace7c529:
#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 "Servo.h" //#include "QEI.h" #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 25 #define pin_cylinder_on p17 #define pin_cylinder_off p18 #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 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); Ticker output_timer; Mecanum mecanum; Bno055 bno055; Position_pid yaw_pid; Accel v1; Accel v2; Accel v3; 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); Servo servo_reload(pin_servo_reload); DigitalIn interrupt_reload(pin_interrupt_reload); void setup(); void output(); void motor_cal(); void cylinder_cal(); 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("rise state : %d\r\n",riseEvent(sbdbt.right)); } } void setup() { wait(1); bno055.begin(); wait(1); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); cylinder_origin(); output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); mecanum.setupdeg(bno055.getYawRad()+180.0); 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); } void output() { motor_cal(); cylinder_cal(); sword_cal(); //boost(); 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)); counter++; break; case 1: rs422.put(id[counter], v2.duty(mecanum.v2()*powerdown), v4.duty(mecanum.v4()*powerdown)); 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(),shoulder_right_cal()); counter ++; break; case 4: rs422.put(id[counter], ((float)sword.getState()*0.8),0.0); counter = 0; break; default: break; }; } 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_cylinder_min == 1) { led1 = 1; 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,180.0,360.0}; void 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_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()); //リロード機構完成後 /* 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) { mecanum.boost_forward(); } if(sbdbt.l2) { mecanum.boost_back(); } /* if(sbdbt.shikaku) { mecanum.boost_left(); } if(sbdbt.maru) { mecanum.boost_right(); } */ } void motor_cal() { yaw = bno055.getYawRad(); target_yaw = yaw; 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()); }