![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
first publish
Diff: main.cpp
- Revision:
- 0:ce3ac53af6e4
- Child:
- 2:359f1f075c72
diff -r 000000000000 -r ce3ac53af6e4 main.cpp --- /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); + + } +} +