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);