![](/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:
- 10:04f2a82cfd89
- Parent:
- 9:6486f4b3ac50
- Child:
- 12:1fec80ae8a2c
--- a/main.cpp Tue Jul 11 09:39:40 2017 +0000 +++ b/main.cpp Sun Jul 16 04:06:49 2017 +0000 @@ -9,6 +9,7 @@ #include "pid.h" #include "limit.h" #include "accelerator.h" +#include "encorder.h" #define pc_baud 460800 #define sbdbt_tx p13 @@ -22,13 +23,23 @@ #define pi 3.141592 #define n1_id 3 #define n2_id 4 -#define n3_id 0 +#define n3_id 5 #define yaw_Kp 0.01 #define yaw_Ki 0.01 #define yaw_Kd 0.01 -#define acceleration 5 +#define acceleration 25 +#define pin_cylinder_on p17 +#define pin_cylinder_off p18 +#define interrupt p16 +#define encoder_cylinder_A p25 +#define encoder_cylinder_B p26 +#define enc_Kp 0.01 +#define enc_Ki 0.01 +#define enc_Kd 0.01 DigitalOut led(LED1); +DigitalOut sylinder_on(pin_cylinder_on); +DigitalOut sylinder_off(pin_cylinder_off); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); Sbdbt sbdbt(sbdbt_tx, sbdbt_rx); @@ -40,11 +51,13 @@ Accel v2; Accel v3; Accel v4; - +Encoder enc_cylinder(encoder_cylinder_A,encoder_cylinder_B); void setup(); void output(); void put_output(); +void cylinder_check(); +void boost(); float yaw, target_yaw; int main() @@ -65,13 +78,47 @@ rs422.begin(rs422_baud); output_timer.attach(&output, output_period); yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd); - mecanum.setupdeg(bno055.getYawRad()); + 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(1200); + enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd); } +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() +{ + if(sbdbt.sankaku) { + sylinder_on = 1; + } else { + sylinder_on = 0; + } + + if(sbdbt.shikaku) { + sylinder_off = 1; + } else { + sylinder_off = 0; + } +} + + + void put_output() { yaw = bno055.getYawRad(); @@ -84,13 +131,12 @@ void output() { put_output(); + //cylinder_check(); + boost(); static int counter; int id[nucleo_num] = {n1_id, n2_id, n3_id}; - if(sbdbt.batu) { - mecanum.boost(); - } switch (counter) { case 0: @@ -102,7 +148,7 @@ counter ++; break; case 2: - rs422.put(id[counter], 0.0, 0.0); + rs422.put(id[counter], sbdbt.right_y, 0.0); counter = 0; break; default: