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
Diff: main.cpp
- Revision:
- 56:a7bd860b85b6
- Parent:
- 55:2dd2f161ebaf
- Child:
- 57:68df771fd8a1
--- a/main.cpp Sat Sep 16 05:59:00 2017 +0000 +++ b/main.cpp Sat Sep 16 07:56:19 2017 +0000 @@ -1,3 +1,4 @@ +//index_include #include "mbed.h" #include "math.h" #include "bit_test.h" @@ -15,6 +16,7 @@ #include "cylinder.h" #include "varEvent.h" +//index_define #define pc_baud 460800 #define sbdbt_tx p13 #define sbdbt_rx p14 @@ -55,8 +57,9 @@ #define pin_cylinder_reload p15 #define pin_sbdbt_pairing p12 #define pin_sbdbt_indicator p11 -#define cylinder_pos_max 4 +#define cylinder_pos_max 6 +//index_setupPin DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); @@ -87,6 +90,7 @@ DigitalOut sbdbt_indigator(pin_sbdbt_indicator); varEvent cylinder_event; +//index_setupFunction void setup(); void output(); void motor_cal(); @@ -99,6 +103,7 @@ float shoulder_right_cal(); float shoulder_left_cal(); +//index_setupVariable //output float yaw, target_yaw; //cylinder_origin @@ -106,7 +111,7 @@ //cylinder float cylinder_pwm; int cylinder_pos_num = 0; -float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,700.0}; +float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,450.0,580.0,700.0}; int main() { @@ -269,10 +274,18 @@ //mecanum void motor_cal() { + static float out_right_x; 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, sbdbt.right_x, bno055.getYawRad()); + 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() @@ -283,10 +296,8 @@ servo_out(); cylinder_origin(); //boost(); - + sbdbt_indigator = sbdbt.get_pairingState(); //led1 = sbdbt.get_pairingState(); - sbdbt_indigator = sbdbt.get_pairingState(); - if(sbdbt.up)cylinder_origin_flag = 1; if(sbdbt.left)bno055.yaw_origin(); cylinder_reload.cyclic(sbdbt.down);