![](/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:
- 7:c4ae1d001d09
- Parent:
- 5:6efda58ff71b
diff -r acefbf83ffe1 -r c4ae1d001d09 main.cpp --- a/main.cpp Fri Jun 30 05:52:04 2017 +0000 +++ b/main.cpp Sat Jul 01 10:12:46 2017 +0000 @@ -9,6 +9,7 @@ #include "pid.h" #include "limit.h" #include "accelerator.h" +#include "encoder.h" #define pc_baud 460800 #define sbdbt_tx p13 @@ -27,6 +28,8 @@ #define yaw_Ki 0.01 #define yaw_Kd 0.01 #define acceleration 5 +#define encoderA p21 +#define encoderB p22 DigitalOut led(LED1); Serial pc(USBTX,USBRX); @@ -40,11 +43,13 @@ Accel v2; Accel v3; Accel v4; +Encoder encoder(encoderA, encoderB); void setup(); void output(); void put_output(); float yaw, target_yaw; +long pulse; int main() { @@ -69,10 +74,13 @@ v2.setup(acceleration,output_period); v3.setup(acceleration,output_period); v4.setup(acceleration,output_period); + encoder.setup(360); } void put_output() { + pulse = encoder.pulse(); + pc.printf("pulse %ld",pulse); yaw = bno055.getYawRad(); target_yaw = yaw; yaw_pid.cal(target_yaw, yaw, output_period);