Revision 11:1b838130dc8c, committed 2016-12-12

Comitter:

aolgu003

Date:

Mon Dec 12 04:29:24 2016 +0000

Parent:

10:8cd741a65646

Commit message:

Added comments

Changed in this revision

diff -r 8cd741a65646 -r 1b838130dc8c main.cpp
--- a/main.cpp	Sat Jul 30 21:32:40 2016 +0000
+++ b/main.cpp	Mon Dec 12 04:29:24 2016 +0000
@@ -8,7 +8,12 @@
 {  
     Timer t;
     wait(3);
+    // Creates a vessel object called seagoat
+    // The vessel object constains the controller, sensor updates, and reads
+    // command sent from PC
     Vessel seagoat; //Starts the seagoat
+    
+    // This section initializes PID gain
     seagoat.SetYawPID(1,0,0);  
     seagoat.SetRollPID(1,0,0);    
     seagoat.SetPitchPID(1,0,0);    

diff -r 8cd741a65646 -r 1b838130dc8c vessel.h
--- a/vessel.h	Sat Jul 30 21:32:40 2016 +0000
+++ b/vessel.h	Mon Dec 12 04:29:24 2016 +0000
@@ -14,6 +14,16 @@
 
 #define BUFFER_SIZE 255
 
+/* Thee
+This is the vessel class this class is meant to be the top level function for 
+sub. 
+The functionalities are as follows:
+    - Initialize sensors and PID control loops
+    - update:
+        - Update sensor values
+        - Then updates controller values
+        - mix controller output to actuator output
+*/
 class Vessel
 {
 
@@ -66,8 +76,12 @@
         IMUPrintData(mpu6050);
         runningTime.start();
     }
-
+    ///////////////////////////////////////////////////////////////////////////
     //Initialise all of the vessels starting parameters
+    // <axis>In = input to PID loop
+    // <axis>Out = output from PID loop
+    // <axis>Point = is the setpoint aka the desired output of PID loop
+    ////////////////////////////////////////////////////////////////////////////
     Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
         pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
         pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
@@ -100,7 +114,8 @@
         pidd.SetOutputLimits(-255,255);
         wait(0.5);
        dPoint = depth;
-
+        
+        // .5 is null .5> is reverse and forward is >.5
         m0 = 0.5;
         m1 = 0.5;
         m2 = 0.5;
@@ -116,7 +131,10 @@
         Start_IMU();
         pc.printf("Seagoat Initialized \n\r");
     }
-
+    
+    ////////////////////////////////////////////////////////////////////////////
+    // Functions for setting PID gains
+    ////////////////////////////////////////////////////////////////////////////
     void SetYawPID(double Kp, double Ki, double Kd) {
         pidy.SetTunings(Kp, Ki, Kd);
     }
@@ -143,7 +161,10 @@
     void SetdPID(double Kp, double Ki, double Kd) {
         pidd.SetTunings(Kp, Ki, Kd);
     }
+/////////////////////////////////////////////////////////////////////////////////
 
+
+    // Callibrates IMU
     void calibrate() {
         IMUUpdate(mpu6050);
         pc.printf("Calibrating...\n\r");
@@ -152,13 +173,30 @@
     }
 
     void update() {
-        //Update IMU Values
+        
+        ///////////////////////////////////////////////
+        //Update IMU Values and store values them in:
+        //    ax, ay, az, gx, gy, gz, yaw, pitch, roll 
+        // (a<axis> and g<axis> are raw sensor values and y, p, r are madgwick values
+        ////////////////////////////////////////////////
         IMUUpdate(mpu6050);
+        ////////////////////////////////////////////////
 
-        //pressure_sensor.Barometer_MS5837();
+
+        //////////////////////////////////////////////////////////////////////
+        // CAUTION max centimeter level resolution takes a really long time to 
+        // calculate. Reduce sensor resolution if you want to increase the update
+        // rate. Alternatively you can offload the pressure measurement to another
+        // MCU or a RTOS can be used so that the updates don't affect other 
+        // processes. Note that this version of mbed that was used to code this
+        // does not support i2c. If a i2c read is called in a thread the MCU will
+        // lock up
+        ////////////////////////////////////////////////////////////////////////
+        //pressure_sensor.Barometer_MS5837(); 
         //depth = pressure_sensor.MS5837_Pressure();
         //pc.printf("Pressure: %f %f\n", depth, dPoint);
-
+        ////////////////////////////////////////////////////////////////////////   
+    
         //Detect if the switch is turned on
         if(!motorState && powerPin.read() == 1) {
             runningTime.stop();
@@ -179,6 +217,7 @@
             motorState = 0;
         }
 
+        // Sets sensor values as Input (feedback) to the PID loops
         yawIn = yaw;
         rollIn = roll;
         pitchIn = pitch;
@@ -186,7 +225,7 @@
         yIn = ay;
         zIn = az;
 
-        //Calculate PID values
+        //Calculate PID output values
         pidy.Compute();
         pidr.Compute();
         pidp.Compute();
@@ -223,15 +262,25 @@
         float forwardThrust = 100;
 
         //Values used in Dynamic Magnitude Calculations
-        float accxs = xOut * xOut * abs(xOut) / xOut;
-        float accys = yOut * yOut * abs(yOut) / yOut;
-        float acczs = zOut * zOut * abs(zOut) / zOut;
-        float depths = dOut * dOut;
-        float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
-        float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
-        float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
+        // I modified this section of marcos code because he was essentially
+        // squaring the output of the PID loops. I don't know why he squares it
+        // I think the same thing can be accomplished by just taking the absolute
+        // values of the output... I highly recommend removing this step. I am
+        // just keeping it in so you can see what we did.
+        float accxs = xOut ^ 2;
+        float accys = yOut ^ 2;
+        float acczs = zOut ^ 2;
+        float depths = dOut ^ 2;
+        float yaws = yawOut ^ 2;
+        float pitchs = pitchOut ^ 2; 
+        float rolls = rollOut ^ 2;
 
         //Values used for Influence calculations
+        // This is part of the method marco used for Pitch and Roll control
+        // I would recomend looking at how quadcopter controllers are designed.
+        // You can see example code here:
+        //      https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp
+        // 
         float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
         float yy  = (abs(yOut) + abs(yawOut)) * 255;
         float xy  = (abs(xOut) + abs(yawOut)) * 255;
@@ -239,9 +288,16 @@
         if (abs(zpr)<255) zpr = 255;
         if (abs(yy)<255) yy = 255;
         if (abs(xy)<255) xy = 255;
-
+        /////////////////////////////////////////////////////////////////////////
+        // Running state 0 just moves forward
+        // Running state 1 moves forward then trys to do a 180 just before the 
+        // navigation gate. Running state is toggled every time the main power 
+        // switch is toggled. 
+        // Also this sections mixes the desired control response to the proper acuators.
+        ////////////////////////////////////////////////////////////////////////
         if(runningState == 0) {
                 if(runningTime < 1) {
+                    // Set PID gain
                     SetYawPID(1,0,0);
                     SetRollPID(1,0,0);
                     SetPitchPID(1,0,0);
@@ -370,7 +426,8 @@
                     neutralizeMotors();
                 }
             }
-
+            ////////////////////////////////////////////////////////////////////
+            // Debugging output ////////////////////////////////////////////////
             //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
             //pc.printf("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), ay, yaws, pitchs, rolls, yy, accys, (-accys + yaws - (forwardThrust * forwardThrust)) / -yy + 0.5);
             //pc.printf("YAW: %f, %f, %f, %f, %f, %f, %f, %f\n\r", abs((acczs + pitchs + rolls) / zpr),abs((accys + yaws) / yy),abs((acczs + pitchs - rolls) / zpr),abs((accxs + yaws) / xy),abs((acczs - pitchs - rolls) / zpr),abs((accys + yaws) / yy),abs((acczs - pitchs + rolls) / zpr),abs((accxs + yaws) / yy));
@@ -378,12 +435,16 @@
             //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
         }
         
+        // Handles rotational singularities (could also use quaternions...)
         float correctAngle(float angle){
             if(angle > 180) return (angle - 360);
             else if(angle < -180) return (angle + 360); 
             else return angle;  
         }
-            
+        
+        // Update command reads a string from the pc via uart. This wasn't used
+        // during the competition because we didn't have a PC. I don't rember if
+        // still functions properly. 
         void updateCommand() {
             //char a = 0;
 //            char i = 0;