Rogelio Vazquez / Mbed 2 deprecated Robosub_test

first publish

Dependencies:   HMC5883L mbed

main.cpp@3:394c971eab83, 2017-06-04 (annotated)

Committer:
roger_wee
Date:
Sun Jun 04 06:58:45 2017 +0000
Revision:
3:394c971eab83
Parent:
2:359f1f075c72 
9-dof implementation using madgwick's filter

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 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