![](/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-07-17
- Revision:
- 13:dfae731e239f
- Parent:
- 12:1fec80ae8a2c
- Child:
- 14:aac2f18f6779
File content as of revision 13:dfae731e239f:
#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 "cyclick.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 3 #define pi 3.141592 #define n1_id 3 #define n2_id 4 #define n3_id 5 #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 p23 #define encoder_A p25 #define encoder_B p26 #define enc_Kp 0.01 #define enc_Ki 0.01 #define enc_Kd 0.01 DigitalOut led1(LED1); DigitalIn interrupt(pin_interrupt); 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; Cyclic_IO led2(LED2); Cyclic_IO sylinder_on(pin_cylinder_on); Cyclic_IO sylinder_off(pin_cylinder_off); //追加点 Encoder enc_cylinder(encoder_A,encoder_B); //QEI wheel(encoder_A, encoder_B, NC, 624); void setup(); void output(); void put_output(); void cylinder_check(); void boost(); void cylinder_origin(); float yaw, target_yaw; int main() { setup(); while(1) { led1 = interrupt; //pc.printf("Pulses is: %i\r\n",wheel.getPulses()); } } void setup() { wait(1); bno055.begin(); wait(1); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); 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); //cylinder_origin(); //追加点 enc_cylinder.setup(1200); enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); } void cylinder_origin(){ while(interrupt){ rs422.put(5, -0.8, 0.0); } } void boost(){ if(sbdbt.sankaku) { mecanum.boost_forward(); } if(sbdbt.batu) { mecanum.boost_back(); } if(sbdbt.shikaku) { mecanum.boost_left(); } if(sbdbt.maru) { mecanum.boost_right(); } } void cylinder_check() { //printf("up\t%d\tstate\t%d\tdown\t%d\tstate\t%d\t\r\n",sbdbt.up,sylinder_on.getState(),sbdbt.down,sylinder_off.getState()); /* if(sylinder_on?){ sylinder_on.cyclic(sbdbt.up); }else if(){ sylinder_off.cyclic(sbdbt.down); } led2.cyclic(sbdbt.left); */ } void put_output() { 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()); } void output() { put_output(); cylinder_check(); boost(); static int counter; int id[nucleo_num] = {n1_id, n2_id, n3_id}; switch (counter) { case 0: rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3())); counter++; break; case 1: rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4())); counter ++; break; case 2: rs422.put(id[counter], sbdbt.right_y, 0.0); counter = 0; break; default: break; }; }