Diff: main.cpp

Revision:

0:ce3ac53af6e4

Child:

2:359f1f075c72

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 15 21:21:08 2017 +0000
@@ -0,0 +1,45 @@
+#include "IMU.h"
+#include "PID.h"
+//#include "MS5837.h"
+#include "Motor.h"
+
+//Declare IMU object
+MPU6050 mpu1;
+
+double myPitch, sOut, setPoint;
+double k_p, k_i, k_d;
+
+Motor mBlack(D3,D2,D9); // pwm, fwd, rev
+Motor mWhite(D4,D5,D8);
+
+//Input, Output, SetPoint, kp, ki, kd, Controller Direction
+PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);
+
+int main()
+{
+    i2c.frequency(400000);  // use fast (400 kHz) I2C
+    wait(.2);
+    //Initialize mpu]
+    IMUinit(mpu1);
+    pidp.SetMode(AUTOMATIC);    
+    pidp.SetOutputLimits(0.5, 1);
+    pidp.SetTunings(.028, 0.01, 0.025);
+    setPoint = 0;
+    
+    while(1)
+    {    
+        //other stuff .. ie state machines  
+        IMUPrintData(mpu1);
+        myPitch = pitch;
+        pidp.Compute();
+        float s2Out = 1.5 - sOut;
+        mWhite.speed(s2Out); 
+        mBlack.speed(-sOut);
+        
+        char text[60];
+        sprintf(text, "%f,%f,%f,%f,%f \n", yaw, pitch, roll,sOut, s2Out);
+        pc.printf("%s", text);
+        
+    }
+}
+