Revision 1:3f291f2f80d3, committed 2017-07-27

Comitter:

roger_wee

Date:

Thu Jul 27 05:45:10 2017 +0000

Parent:

0:048a74dd7c3a

Commit message:

control edit

Changed in this revision

diff -r 048a74dd7c3a -r 3f291f2f80d3 Sensors/IMU.h
--- a/Sensors/IMU.h	Sat Jul 22 05:58:03 2017 +0000
+++ b/Sensors/IMU.h	Thu Jul 27 05:45:10 2017 +0000
@@ -117,7 +117,7 @@
     gz *= PI/180.0f;
     
     // Calculate position if time
-    mpu6050.getDisplacement(ax,ay);
+    //mpu6050.getDisplacement(ax,ay);
 
     // Pass gyro rate as rad/s
     mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, magdata[0], magdata[1], magdata[2]);

diff -r 048a74dd7c3a -r 3f291f2f80d3 main.cpp
--- a/main.cpp	Sat Jul 22 05:58:03 2017 +0000
+++ b/main.cpp	Thu Jul 27 05:45:10 2017 +0000
@@ -6,30 +6,58 @@
 #include "HMC5883L.h"
 
 //------------Declare Objects------------------//
+
+//      (m1)         (m2)
+//          |       |
+//          |       |
+// (Ltrhust)|       |(Rthrust)
+//          |       |
+//          |       |    
+//      (m3)         (m4)   
+
 PwmOut      m1(D3);
 PwmOut      m2(D4);
 PwmOut      m3(D5);
 PwmOut      m4(D6);
 
+PwmOut      Lthrust(D7);
+PwmOut      Rthrust(D8);
+
 // Declare mpu6050 and compass object
 MPU6050 mpu1;
 HMC5883L compass;
 
-// Serial communication between Arduino NANO
+// Serial communication between STM & Arduino NANO
 RawSerial device(PA_11, PA_12);  //TX RX
 
+// Serial communication between STM & Raspberry PI
+RawSerial devicePI(D10,D2);
+
 //-----------Global Variables------------------//
 char command = 0;
 Timer calibrate;
+Timer data;
 
 int pwmMax = 1100;      // Reversed due to motor orientation
 int pwmMin = 1500;
 
+const int IDLE = 1500;
+const int MAXYAW = 100;
+const int MAXPITCH = 100;
+const int MAXROLL = 100;
+
+int thrust = 0;
+
+int j = -1;      // j == 1 --> FWD ... j == -1 --> REV      
+
+
 // PWM output variable for each motor
 int m1pwm = pwmMin;
 int m2pwm = pwmMin;
 int m3pwm = pwmMin;
 int m4pwm = pwmMin;
+int Lthrustpwm = pwmMin;
+int Rthrustpwm = pwmMin;
 
 // Individual pid parameters
 double myYaw, yawPoint, yawOut;
@@ -40,6 +68,8 @@
 int depth = 1;
 
 double kpVal = 0;
+double kiVal = 0;
+double kdVal = 0;
 
 //-----------End Global Variables--------------//
 
@@ -59,7 +89,7 @@
 void calibrateFilter()
 {
     calibrate.start();
-    while(calibrate.read() < 10)
+    while(calibrate.read() < 5)
     {
         IMUPrintData(mpu1, compass);
         
@@ -76,6 +106,8 @@
     m2.pulsewidth_us(m2pwm);
     m3.pulsewidth_us(m3pwm);
     m4.pulsewidth_us(m4pwm);
+    Lthrust.pulsewidth_us(Lthrustpwm);
+    Rthrust.pulsewidth_us(Rthrustpwm);
 }
 
 void neutralizeMotors()
@@ -84,9 +116,20 @@
     m2pwm = pwmMin;
     m3pwm = pwmMin;
     m4pwm = pwmMin;
+    Lthrustpwm = pwmMin;
+    Rthrustpwm = pwmMin;
     updateMotors(); 
 }
 
+void vesselGo()
+{
+    thrust = 200;   
+}
+void vesselHover()
+{
+    thrust = 0;
+}
+
 void readUserInput()
 {
     if(pc.readable())
@@ -95,21 +138,29 @@
     }  
 }
 
+void updateYawThrottle()
+{
+    Lthrustpwm = IDLE + (j*(thrust + yawOut));
+    Rthrustpwm = IDLE + (j*(thrust + (MAXYAW-yawOut)));
+}
+
 void initializePIDs()
 {
     pidy.SetMode(AUTOMATIC);            // Yaw PID
-    pidy.SetOutputLimits(pwmMin, pwmMax);
+    pidy.SetOutputLimits(0, MAXYAW);
     
     pidp.SetMode(AUTOMATIC);            // Pitch PID
-    pidp.SetOutputLimits(pwmMin, pwmMax); 
+    pidp.SetOutputLimits(0, MAXPITCH); 
     
     pidr.SetMode(AUTOMATIC);            // Roll PID
-    pidr.SetOutputLimits(pwmMin, pwmMax);
+    pidr.SetOutputLimits(0, MAXROLL);
     
     pidd.SetMode(AUTOMATIC);            // Depth PID
-    pidd.SetOutputLimits(pwmMin, pwmMax); 
+    pidd.SetOutputLimits(0,300); 
     
     depthPoint = 0;
+    pitchPoint = 0;
+    rollPoint = 0;
 }
 
 void readDepth()
@@ -127,7 +178,7 @@
 void displayTelemetry()
 {
     char text[90];
-    sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth);
+    sprintf(text, "%f,%f,%f,%d,%f \n", yaw, pitch, roll, depth, depthPoint);
     pc.printf("%s", text);
 }
 
@@ -147,7 +198,7 @@
             IMUinit(mpu1);
             compass.init();
             
-            pc.printf("Initialize motors  \n");
+            pc.printf("Initialirze motors  \n");
             neutralizeMotors();
             
             pc.printf("Initialize PID objects \n");
@@ -198,23 +249,46 @@
                 break;
             }
             updateMotors();
+            displayTelemetry();
         break;
         
         case preparePid:
            // pc.printf("Prepare PID \n");
             switch(command)
             {
-                case 'I':           // Increase Kp gain
+                case 'K':           // Increase Kp gain
                     kpVal += .1;
                 break;
                 
-                case 'R':
+                case 'k':
                     if (kpVal > 0 ) // Decreae Kp gain
                     {
                         kpVal -= .1;  
                     }
                 break;
                 
+                case 'I':
+                    kiVal += .1;
+                break;
+                
+                case 'i':
+                    if (kiVal > 0)
+                    {
+                        kiVal -= .1;
+                    }
+                break;
+                
+                case 'D':
+                    kdVal += .1;
+                break;
+                
+                case 'd':
+                    if (kdVal > 0)
+                    {
+                        kdVal -= .1;
+                    }                    
+                break;
+                
                 case 'S':           // Increase depth
                     depthPoint++;
                 
@@ -230,12 +304,19 @@
             }
             
             // Set tunings
-            pidd.SetTunings(kpVal,0,0);     //Set Ki Kd = 0 
+            pidd.SetTunings(kpVal,kiVal,kdVal);     //Set Ki Kd = 0 
+            //pidy.SetTunings(kpVal,kiVal,kdVal);
+            //pidp.SetTunings(kpVal,kiVal,kdVal);
+            //pidr.SetTunings(kpVal,kiVal,kdVal);
             
-            // Print Set parameters
-            pc.printf("DepthPoint: %d \n", depthPoint);
-            pc.printf("Kp gain: %d \n", pidd.GetKp());
-            
+            if (data.read_ms() % 500 == 0)
+            {
+                // Print Set parameters
+                pc.printf("DepthPoint: %d \n", depthPoint);
+                pc.printf("Kp gain: %d \n", pidd.GetKp());
+                pc.printf("Ki gain: %d \n", pidd.GetKi());
+                pc.printf("Kd gain: %d \n", pidd.GetKd());
+            }
         break;
         
         case beginTune:
@@ -247,12 +328,23 @@
             
             // Assign feedback variables
             myDepth = depth;
+            //myYaw = yaw;
+            //myPitch = pitch;
+            //myRoll = Roll;
             
             // Compute PID
             pidd.Compute();
+            //pidy.Compute();
+            //pidp.Compute();
+            //pidr.Compute();
             
             // Output pwm to motors                     // FIXME : account for pitch, roll and maybe yaw depending on motor configuration
-            m1pwm = m2pwm = m3pwm = m4pwm = depthOut;
+            m1pwm = IDLE + depthOut; // + pitchOut 
+            m2pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
+            m3pwm = IDLE + depthOut; // + pitchOut
+            m4pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
+            
+            //updateYawThrottle();
             updateMotors();
             
             // Display telemetry
@@ -307,7 +399,7 @@
 int main() {
     // Initialize control state
     controlState = init;
-    
+    data.start();
     while(1)
     {
         // Read user input