![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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:
- 2:d5b8f8e62923
- Parent:
- 1:2d878962e6ea
- Child:
- 3:9ef929639c4a
--- a/main.cpp Thu Jun 29 08:52:40 2017 +0000 +++ b/main.cpp Thu Jun 29 09:47:49 2017 +0000 @@ -21,9 +21,9 @@ #define n1_id 3 #define n2_id 4 #define n3_id 0 -#define Kp 0.0 -#define Ki 0.0 -#define Kd 0.0 +#define yaw_Kp 0.07 +#define yaw_Ki 0 +#define yaw_Kd 0 DigitalOut led(LED1); Serial pc(USBTX,USBRX); @@ -32,18 +32,24 @@ Ticker output_timer; Mecanum mecanum; Bno055 bno055; +Position_pid yaw_pid; + void setup(); void output(); void put_output(); float m1, m2, m3, m4; float a; +float yaw, target_yaw; int main() { setup(); - while(1) { - mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, 0, bno055.getYawRad()); + while(1) { + 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()); put_output(); } @@ -59,6 +65,7 @@ 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()); }