Diff: main.cpp

Revision:

4:a51fa881cc4c

Parent:

2:359f1f075c72

--- a/main.cpp	Sun Jun 04 06:58:45 2017 +0000
+++ b/main.cpp	Mon Feb 12 07:27:47 2018 +0000
@@ -1,6 +1,5 @@
 #include "IMU.h"
 #include "PID.h"
-#include "MS5837.h"
 #include "Motor.h"
 #include "HMC5883L.h"
 
@@ -11,69 +10,124 @@
   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;
+
+DigitalOut  my_led(LED1);
 
-//Declare digital button input
-DigitalIn tuningButton(USER_BUTTON);
+// Individual pid parameters
+double myYaw, yawPoint, yawOut;
+double myPitch, pitchPoint, pitchOut;
+double myRoll, rollPoint, rollOut;
+double myDepth, depthPoint, depthOut;
 
+//Declare digital button input                  tuning parameters   //FIXME
+DigitalIn tuningButton(USER_BUTTON);            
 
-// Declare mpu object
+// Declare mpu6050 and compass object
 MPU6050 mpu1;
 HMC5883L compass;
 
-MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS);
+// Declare analog input pin ( kp, ki, kd )      tuning parameters
+AnalogIn    tuneKnob(A0);
 
-// Declare motor objects
-Motor mBlack(D3,D2,D9); // pwm, fwd, rev
-Motor mWhite(D4,D5,D8);
+// Declare motor pins
+PwmOut      m1(D3);
+PwmOut      m2(D4);
+PwmOut      m3(D5);
+PwmOut      m4(D6);
 
-// Declare analog input pin 
-AnalogIn    kpKnob(A0);
-AnalogIn    kiKnob(A1);
-AnalogIn    kdKnob(A2);
+// PWM output variable for each motor
+int m1pwm, m2pwm, m3pwm, m4pwm;
 
 // Input, Output, SetPoint, kp, ki, kd, Controller Direction
-PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);
+PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT);
+PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT);
+PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT);
+PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT);
 
 //Serial pc(USBTX, USBRX);
 
+// Serial communication between Arduino NANO
+RawSerial device(PA_11, PA_12);  //TX RX
+
+Timer calibrate;
+
+int depth = 1;
+
 int main()
 {   
     // Initialize IMU
     IMUinit(mpu1);
+    
+    // Initialize Magnetometer
     compass.init();
-    // Initialize pressure sensor
- //   pressure_sensor.MS5837Init();
+    
+    // Initialize PID's 
+    pidy.SetMode(AUTOMATIC);            // Yaw PID
+    pidy.SetOutputLimits(1500, 1700);
     
-    // Initialize PID 
-    pidp.SetMode(AUTOMATIC);    
-    pidp.SetOutputLimits(0.5, 1);
+    pidp.SetMode(AUTOMATIC);            // Pitch PID
+    pidp.SetOutputLimits(1500, 1700); 
+    
+    pidr.SetMode(AUTOMATIC);            // Roll PID
+    pidr.SetOutputLimits(1500, 1700);
     
-    //Default kp ki kd parameters
+    pidd.SetMode(AUTOMATIC);            // Depth PID
+    pidd.SetOutputLimits(1500, 1700); 
+
+    
+    // Default kp ki kd parameters
     float kpKnobVal = .028;
     float kiKnobVal = .01;
     float kdKnobVal = .025;
-    pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
-    setPoint = 0;
+    
+    // Configure PID's
+    pidd.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
+    depthPoint = 0;
     
-   // float pressure;
+    //Calibrate IMU
+    calibrate.start();
+    while(calibrate.read() < 5);
+    {
+        IMUPrintData(mpu1, compass);
+        my_led = 1;
+    }
+    my_led = 0;
+    
+    calibrate.stop();
 
     while(1)
     {    
+        //-------------------------SENSE--------------------------------
     
-        // Read pressure sensor data
-       // pressure_sensor.Barometer_MS5837();
-       // pressure = pressure_sensor.MS5837_Temperature();
-       // pc.printf("pressure: %f \n", pressure);
+        // Read pressure sensor data if available
+        if (device.readable())
+        {
+            // Receive depth in inches as an integer
+            depth = device.getc();
+            
+            // Convert to feet
+            
+            // Display received data
+           // pc.printf("%d \n", pressure);
+        }
+        
+        // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
+        IMUPrintData(mpu1, compass);
+        
+        // Assign feedback variables
+        myDepth = depth;
+        
+        //------------------------End SENSE----------------------------
+        
+        
+        //------------------------Tuning ADJUST-------------------------    FIXMEEEEEE
         
         // 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);
+            kpKnobVal = map(tuneKnob.read(), 0.000, 1.000, 0.000, .050);
+           
             
             // Adjust tunings
             pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal);
@@ -82,28 +136,35 @@
         //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;
+        //------------------------End tuning ADJUST-----------------------
         
-       // char textA[90];
-       // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading);
-       // pc.printf("%s", textA);
+        
+        //------------------------Motor Control LOOP----------------------
         
         // Compute output using pid controller
-        //pidp.Compute();
+        pidd.Compute();
+        
+        // Assign pid output pwm to individual pwm variables
+        m1pwm = depthOut;
+        m2pwm = depthOut;
+        m3pwm = depthOut;
+        m4pwm = depthOut;
         
-        // Send pwm output to Motors
-        //float s2Out = 1.5 - sOut;
-        //mWhite.speed(s2Out); 
-        //mBlack.speed(-sOut);
-                
-        //Display telemetry
+        // Output pwm
+        m1.pulsewidth_us(m1pwm);
+        m2.pulsewidth_us(m2pwm);
+        m3.pulsewidth_us(m3pwm);
+        m4.pulsewidth_us(m4pwm);
+        
+        //------------------------End Motor Control LOOP-------------------    
+        
+        
+        //------------------------Display TELEMETRY------------------------
         char text[90];
-        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
+        sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth);
         pc.printf("%s", text);
+        //--------------------------End TELEMETRY--------------------------
         
-        wait(.01);
     }
 }