Script for a drone PID tuning interface using buttons, potentiometers and a 3-D printed platform.
Fork of Robosub_test by
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?
User | Revision | Line number | New 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 |