Diff: main.cpp

Revision:

2:359f1f075c72

Parent:

0:ce3ac53af6e4

--- a/main.cpp	Mon May 15 21:26:09 2017 +0000
+++ b/main.cpp	Sun Jun 04 06:46:28 2017 +0000
@@ -1,45 +1,109 @@
 #include "IMU.h"
 #include "PID.h"
-//#include "MS5837.h"
+#include "MS5837.h"
 #include "Motor.h"
+#include "HMC5883L.h"
 
-//Declare IMU object
-MPU6050 mpu1;
+// Maps value from incoming analog signal to correct range
+// to be sent out to as pwm signal to motors
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
 
 double myPitch, sOut, setPoint;
 double k_p, k_i, k_d;
 
+//Declare digital button input
+DigitalIn tuningButton(USER_BUTTON);
+
+
+// Declare mpu object
+MPU6050 mpu1;
+HMC5883L compass;
+
+MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS);
+
+// Declare motor objects
 Motor mBlack(D3,D2,D9); // pwm, fwd, rev
 Motor mWhite(D4,D5,D8);
 
-//Input, Output, SetPoint, kp, ki, kd, Controller Direction
+// Declare analog input pin 
+AnalogIn    kpKnob(A0);
+AnalogIn    kiKnob(A1);
+AnalogIn    kdKnob(A2);
+
+// Input, Output, SetPoint, kp, ki, kd, Controller Direction
 PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);
 
+//Serial pc(USBTX, USBRX);
+
 int main()
-{
-    i2c.frequency(400000);  // use fast (400 kHz) I2C
-    wait(.2);
-    //Initialize mpu]
+{   
+    // Initialize IMU
     IMUinit(mpu1);
+    compass.init();
+    // Initialize pressure sensor
+ //   pressure_sensor.MS5837Init();
+    
+    // Initialize PID 
     pidp.SetMode(AUTOMATIC);    
     pidp.SetOutputLimits(0.5, 1);
-    pidp.SetTunings(.028, 0.01, 0.025);
+    
+    //Default kp ki kd parameters
+    float kpKnobVal = .028;
+    float kiKnobVal = .01;
+    float kdKnobVal = .025;
+    pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
     setPoint = 0;
     
+   // float pressure;
+
     while(1)
     {    
-        //other stuff .. ie state machines  
-        IMUPrintData(mpu1);
-        myPitch = pitch;
-        pidp.Compute();
-        float s2Out = 1.5 - sOut;
-        mWhite.speed(s2Out); 
-        mBlack.speed(-sOut);
+    
+        // Read pressure sensor data
+       // pressure_sensor.Barometer_MS5837();
+       // pressure = pressure_sensor.MS5837_Temperature();
+       // pc.printf("pressure: %f \n", pressure);
         
-        char text[60];
-        sprintf(text, "%f,%f,%f,%f,%f \n", yaw, pitch, roll,sOut, s2Out);
+        // If button is pressed kp ki kd values can be adjusted
+        if(!tuningButton)
+        {
+            // Read raw potentiometer values from k-knob and map to kpknobVal
+            kpKnobVal = map(kpKnob.read(), 0.000, 1.000, 0.000, .050);
+            kiKnobVal = map(kiKnob.read(), 0.000, 1.000, 0.000, 0.008);
+            kdKnobVal = map(kdKnob.read(), 0.000, 1.000, 0.000, .040);
+            
+            // Adjust tunings
+            pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal);
+            
+        }
+        //print mapped joystick values
+       // pc.printf("kp: %f -- ki: %f -- kd %f \n", kpKnobVal, kiKnobVal, kdKnobVal);  
+        
+        // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
+        IMUPrintData(mpu1, compass);
+        //myPitch = pitch;
+        
+       // char textA[90];
+       // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading);
+       // pc.printf("%s", textA);
+        
+        // Compute output using pid controller
+        //pidp.Compute();
+        
+        // Send pwm output to Motors
+        //float s2Out = 1.5 - sOut;
+        //mWhite.speed(s2Out); 
+        //mBlack.speed(-sOut);
+                
+        //Display telemetry
+        char text[90];
+        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
         pc.printf("%s", text);
         
+        wait(.01);
     }
 }