Rogelio Vazquez / Mbed 2 deprecated Robosub_test

first publish

Dependencies:   HMC5883L mbed

main.cpp@0:ce3ac53af6e4, 2017-05-15 (annotated)

Committer:
roger_wee
Date:
Mon May 15 21:21:08 2017 +0000
Revision:
0:ce3ac53af6e4
Child:
2:359f1f075c72
first_commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger_wee 0:ce3ac53af6e4 1 #include "IMU.h"
roger_wee 0:ce3ac53af6e4 2 #include "PID.h"
roger_wee 0:ce3ac53af6e4 3 //#include "MS5837.h"
roger_wee 0:ce3ac53af6e4 4 #include "Motor.h"
roger_wee 0:ce3ac53af6e4 5
roger_wee 0:ce3ac53af6e4 6 //Declare IMU object
roger_wee 0:ce3ac53af6e4 7 MPU6050 mpu1;
roger_wee 0:ce3ac53af6e4 8
roger_wee 0:ce3ac53af6e4 9 double myPitch, sOut, setPoint;
roger_wee 0:ce3ac53af6e4 10 double k_p, k_i, k_d;
roger_wee 0:ce3ac53af6e4 11
roger_wee 0:ce3ac53af6e4 12 Motor mBlack(D3,D2,D9); // pwm, fwd, rev
roger_wee 0:ce3ac53af6e4 13 Motor mWhite(D4,D5,D8);
roger_wee 0:ce3ac53af6e4 14
roger_wee 0:ce3ac53af6e4 15 //Input, Output, SetPoint, kp, ki, kd, Controller Direction
roger_wee 0:ce3ac53af6e4 16 PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);
roger_wee 0:ce3ac53af6e4 17
roger_wee 0:ce3ac53af6e4 18 int main()
roger_wee 0:ce3ac53af6e4 19 {
roger_wee 0:ce3ac53af6e4 20 i2c.frequency(400000); // use fast (400 kHz) I2C
roger_wee 0:ce3ac53af6e4 21 wait(.2);
roger_wee 0:ce3ac53af6e4 22 //Initialize mpu]
roger_wee 0:ce3ac53af6e4 23 IMUinit(mpu1);
roger_wee 0:ce3ac53af6e4 24 pidp.SetMode(AUTOMATIC);
roger_wee 0:ce3ac53af6e4 25 pidp.SetOutputLimits(0.5, 1);
roger_wee 0:ce3ac53af6e4 26 pidp.SetTunings(.028, 0.01, 0.025);
roger_wee 0:ce3ac53af6e4 27 setPoint = 0;
roger_wee 0:ce3ac53af6e4 28
roger_wee 0:ce3ac53af6e4 29 while(1)
roger_wee 0:ce3ac53af6e4 30 {
roger_wee 0:ce3ac53af6e4 31 //other stuff .. ie state machines
roger_wee 0:ce3ac53af6e4 32 IMUPrintData(mpu1);
roger_wee 0:ce3ac53af6e4 33 myPitch = pitch;
roger_wee 0:ce3ac53af6e4 34 pidp.Compute();
roger_wee 0:ce3ac53af6e4 35 float s2Out = 1.5 - sOut;
roger_wee 0:ce3ac53af6e4 36 mWhite.speed(s2Out);
roger_wee 0:ce3ac53af6e4 37 mBlack.speed(-sOut);
roger_wee 0:ce3ac53af6e4 38
roger_wee 0:ce3ac53af6e4 39 char text[60];
roger_wee 0:ce3ac53af6e4 40 sprintf(text, "%f,%f,%f,%f,%f \n", yaw, pitch, roll,sOut, s2Out);
roger_wee 0:ce3ac53af6e4 41 pc.printf("%s", text);
roger_wee 0:ce3ac53af6e4 42
roger_wee 0:ce3ac53af6e4 43 }
roger_wee 0:ce3ac53af6e4 44 }
roger_wee 0:ce3ac53af6e4 45