Rogelio Vazquez
/
Robosub_test
first publish
main.cpp@2:359f1f075c72, 2017-06-04 (annotated)
- Committer:
- roger_wee
- Date:
- Sun Jun 04 06:46:28 2017 +0000
- Revision:
- 2:359f1f075c72
- Parent:
- 0:ce3ac53af6e4
9-dof implementation using madgwick's filter
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 | 2:359f1f075c72 | 3 | #include "MS5837.h" |
roger_wee | 0:ce3ac53af6e4 | 4 | #include "Motor.h" |
roger_wee | 2:359f1f075c72 | 5 | #include "HMC5883L.h" |
roger_wee | 0:ce3ac53af6e4 | 6 | |
roger_wee | 2:359f1f075c72 | 7 | // Maps value from incoming analog signal to correct range |
roger_wee | 2:359f1f075c72 | 8 | // to be sent out to as pwm signal to motors |
roger_wee | 2:359f1f075c72 | 9 | float map(float x, float in_min, float in_max, float out_min, float out_max) |
roger_wee | 2:359f1f075c72 | 10 | { |
roger_wee | 2:359f1f075c72 | 11 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
roger_wee | 2:359f1f075c72 | 12 | } |
roger_wee | 0:ce3ac53af6e4 | 13 | |
roger_wee | 0:ce3ac53af6e4 | 14 | double myPitch, sOut, setPoint; |
roger_wee | 0:ce3ac53af6e4 | 15 | double k_p, k_i, k_d; |
roger_wee | 0:ce3ac53af6e4 | 16 | |
roger_wee | 2:359f1f075c72 | 17 | //Declare digital button input |
roger_wee | 2:359f1f075c72 | 18 | DigitalIn tuningButton(USER_BUTTON); |
roger_wee | 2:359f1f075c72 | 19 | |
roger_wee | 2:359f1f075c72 | 20 | |
roger_wee | 2:359f1f075c72 | 21 | // Declare mpu object |
roger_wee | 2:359f1f075c72 | 22 | MPU6050 mpu1; |
roger_wee | 2:359f1f075c72 | 23 | HMC5883L compass; |
roger_wee | 2:359f1f075c72 | 24 | |
roger_wee | 2:359f1f075c72 | 25 | MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS); |
roger_wee | 2:359f1f075c72 | 26 | |
roger_wee | 2:359f1f075c72 | 27 | // Declare motor objects |
roger_wee | 0:ce3ac53af6e4 | 28 | Motor mBlack(D3,D2,D9); // pwm, fwd, rev |
roger_wee | 0:ce3ac53af6e4 | 29 | Motor mWhite(D4,D5,D8); |
roger_wee | 0:ce3ac53af6e4 | 30 | |
roger_wee | 2:359f1f075c72 | 31 | // Declare analog input pin |
roger_wee | 2:359f1f075c72 | 32 | AnalogIn kpKnob(A0); |
roger_wee | 2:359f1f075c72 | 33 | AnalogIn kiKnob(A1); |
roger_wee | 2:359f1f075c72 | 34 | AnalogIn kdKnob(A2); |
roger_wee | 2:359f1f075c72 | 35 | |
roger_wee | 2:359f1f075c72 | 36 | // Input, Output, SetPoint, kp, ki, kd, Controller Direction |
roger_wee | 0:ce3ac53af6e4 | 37 | PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT); |
roger_wee | 0:ce3ac53af6e4 | 38 | |
roger_wee | 2:359f1f075c72 | 39 | //Serial pc(USBTX, USBRX); |
roger_wee | 2:359f1f075c72 | 40 | |
roger_wee | 0:ce3ac53af6e4 | 41 | int main() |
roger_wee | 2:359f1f075c72 | 42 | { |
roger_wee | 2:359f1f075c72 | 43 | // Initialize IMU |
roger_wee | 0:ce3ac53af6e4 | 44 | IMUinit(mpu1); |
roger_wee | 2:359f1f075c72 | 45 | compass.init(); |
roger_wee | 2:359f1f075c72 | 46 | // Initialize pressure sensor |
roger_wee | 2:359f1f075c72 | 47 | // pressure_sensor.MS5837Init(); |
roger_wee | 2:359f1f075c72 | 48 | |
roger_wee | 2:359f1f075c72 | 49 | // Initialize PID |
roger_wee | 0:ce3ac53af6e4 | 50 | pidp.SetMode(AUTOMATIC); |
roger_wee | 0:ce3ac53af6e4 | 51 | pidp.SetOutputLimits(0.5, 1); |
roger_wee | 2:359f1f075c72 | 52 | |
roger_wee | 2:359f1f075c72 | 53 | //Default kp ki kd parameters |
roger_wee | 2:359f1f075c72 | 54 | float kpKnobVal = .028; |
roger_wee | 2:359f1f075c72 | 55 | float kiKnobVal = .01; |
roger_wee | 2:359f1f075c72 | 56 | float kdKnobVal = .025; |
roger_wee | 2:359f1f075c72 | 57 | pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal); |
roger_wee | 0:ce3ac53af6e4 | 58 | setPoint = 0; |
roger_wee | 0:ce3ac53af6e4 | 59 | |
roger_wee | 2:359f1f075c72 | 60 | // float pressure; |
roger_wee | 2:359f1f075c72 | 61 | |
roger_wee | 0:ce3ac53af6e4 | 62 | while(1) |
roger_wee | 0:ce3ac53af6e4 | 63 | { |
roger_wee | 2:359f1f075c72 | 64 | |
roger_wee | 2:359f1f075c72 | 65 | // Read pressure sensor data |
roger_wee | 2:359f1f075c72 | 66 | // pressure_sensor.Barometer_MS5837(); |
roger_wee | 2:359f1f075c72 | 67 | // pressure = pressure_sensor.MS5837_Temperature(); |
roger_wee | 2:359f1f075c72 | 68 | // pc.printf("pressure: %f \n", pressure); |
roger_wee | 0:ce3ac53af6e4 | 69 | |
roger_wee | 2:359f1f075c72 | 70 | // If button is pressed kp ki kd values can be adjusted |
roger_wee | 2:359f1f075c72 | 71 | if(!tuningButton) |
roger_wee | 2:359f1f075c72 | 72 | { |
roger_wee | 2:359f1f075c72 | 73 | // Read raw potentiometer values from k-knob and map to kpknobVal |
roger_wee | 2:359f1f075c72 | 74 | kpKnobVal = map(kpKnob.read(), 0.000, 1.000, 0.000, .050); |
roger_wee | 2:359f1f075c72 | 75 | kiKnobVal = map(kiKnob.read(), 0.000, 1.000, 0.000, 0.008); |
roger_wee | 2:359f1f075c72 | 76 | kdKnobVal = map(kdKnob.read(), 0.000, 1.000, 0.000, .040); |
roger_wee | 2:359f1f075c72 | 77 | |
roger_wee | 2:359f1f075c72 | 78 | // Adjust tunings |
roger_wee | 2:359f1f075c72 | 79 | pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal); |
roger_wee | 2:359f1f075c72 | 80 | |
roger_wee | 2:359f1f075c72 | 81 | } |
roger_wee | 2:359f1f075c72 | 82 | //print mapped joystick values |
roger_wee | 2:359f1f075c72 | 83 | // pc.printf("kp: %f -- ki: %f -- kd %f \n", kpKnobVal, kiKnobVal, kdKnobVal); |
roger_wee | 2:359f1f075c72 | 84 | |
roger_wee | 2:359f1f075c72 | 85 | // Obtain mpu data -> pass through filter -> obtain yaw pitch roll |
roger_wee | 2:359f1f075c72 | 86 | IMUPrintData(mpu1, compass); |
roger_wee | 2:359f1f075c72 | 87 | //myPitch = pitch; |
roger_wee | 2:359f1f075c72 | 88 | |
roger_wee | 2:359f1f075c72 | 89 | // char textA[90]; |
roger_wee | 2:359f1f075c72 | 90 | // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading); |
roger_wee | 2:359f1f075c72 | 91 | // pc.printf("%s", textA); |
roger_wee | 2:359f1f075c72 | 92 | |
roger_wee | 2:359f1f075c72 | 93 | // Compute output using pid controller |
roger_wee | 2:359f1f075c72 | 94 | //pidp.Compute(); |
roger_wee | 2:359f1f075c72 | 95 | |
roger_wee | 2:359f1f075c72 | 96 | // Send pwm output to Motors |
roger_wee | 2:359f1f075c72 | 97 | //float s2Out = 1.5 - sOut; |
roger_wee | 2:359f1f075c72 | 98 | //mWhite.speed(s2Out); |
roger_wee | 2:359f1f075c72 | 99 | //mBlack.speed(-sOut); |
roger_wee | 2:359f1f075c72 | 100 | |
roger_wee | 2:359f1f075c72 | 101 | //Display telemetry |
roger_wee | 2:359f1f075c72 | 102 | char text[90]; |
roger_wee | 2:359f1f075c72 | 103 | sprintf(text, "%f,%f,%f \n", yaw, pitch, roll); |
roger_wee | 0:ce3ac53af6e4 | 104 | pc.printf("%s", text); |
roger_wee | 0:ce3ac53af6e4 | 105 | |
roger_wee | 2:359f1f075c72 | 106 | wait(.01); |
roger_wee | 0:ce3ac53af6e4 | 107 | } |
roger_wee | 0:ce3ac53af6e4 | 108 | } |
roger_wee | 0:ce3ac53af6e4 | 109 |