![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
first publish
main.cpp
- Committer:
- roger_wee
- Date:
- 2017-05-15
- Revision:
- 0:ce3ac53af6e4
- Child:
- 2:359f1f075c72
File content as of revision 0:ce3ac53af6e4:
#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); } }