Diff: main.cpp

Revision:

0:048a74dd7c3a

Child:

1:3f291f2f80d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jul 22 05:58:03 2017 +0000
@@ -0,0 +1,319 @@
+//Robosub Control Interface
+
+#include "mbed.h"
+#include "IMU.h"
+#include "PID.h"
+#include "HMC5883L.h"
+
+//------------Declare Objects------------------//
+PwmOut      m1(D3);
+PwmOut      m2(D4);
+PwmOut      m3(D5);
+PwmOut      m4(D6);
+
+// Declare mpu6050 and compass object
+MPU6050 mpu1;
+HMC5883L compass;
+
+// Serial communication between Arduino NANO
+RawSerial device(PA_11, PA_12);  //TX RX
+
+//-----------Global Variables------------------//
+char command = 0;
+Timer calibrate;
+
+int pwmMax = 1100;      // Reversed due to motor orientation
+int pwmMin = 1500;
+
+// PWM output variable for each motor
+int m1pwm = pwmMin;
+int m2pwm = pwmMin;
+int m3pwm = pwmMin;
+int m4pwm = pwmMin;
+
+// Individual pid parameters
+double myYaw, yawPoint, yawOut;
+double myPitch, pitchPoint, pitchOut;
+double myRoll, rollPoint, rollOut;
+double myDepth, depthPoint, depthOut;
+
+int depth = 1;
+
+double kpVal = 0;
+
+//-----------End Global Variables--------------//
+
+
+            //----PID objects------//
+
+// Input, Output, SetPoint, kp, ki, kd, Controller Direction
+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);
+
+            //----End PID objects--//
+
+
+//-----------Helper functions------------------//
+void calibrateFilter()
+{
+    calibrate.start();
+    while(calibrate.read() < 10)
+    {
+        IMUPrintData(mpu1, compass);
+        
+        char text[90];
+        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
+        pc.printf("%s", text);
+    }
+    calibrate.stop();    
+}
+
+void updateMotors()
+{
+    m1.pulsewidth_us(m1pwm);
+    m2.pulsewidth_us(m2pwm);
+    m3.pulsewidth_us(m3pwm);
+    m4.pulsewidth_us(m4pwm);
+}
+
+void neutralizeMotors()
+{
+    m1pwm = pwmMin;
+    m2pwm = pwmMin;
+    m3pwm = pwmMin;
+    m4pwm = pwmMin;
+    updateMotors(); 
+}
+
+void readUserInput()
+{
+    if(pc.readable())
+    {
+         command = pc.getc();
+    }  
+}
+
+void initializePIDs()
+{
+    pidy.SetMode(AUTOMATIC);            // Yaw PID
+    pidy.SetOutputLimits(pwmMin, pwmMax);
+    
+    pidp.SetMode(AUTOMATIC);            // Pitch PID
+    pidp.SetOutputLimits(pwmMin, pwmMax); 
+    
+    pidr.SetMode(AUTOMATIC);            // Roll PID
+    pidr.SetOutputLimits(pwmMin, pwmMax);
+    
+    pidd.SetMode(AUTOMATIC);            // Depth PID
+    pidd.SetOutputLimits(pwmMin, pwmMax); 
+    
+    depthPoint = 0;
+}
+
+void readDepth()
+{
+    // Read pressure sensor data if available
+    if (device.readable())
+    {
+        // Receive depth in inches as an integer
+        depth = device.getc();
+            
+        // Convert to feet
+    }
+}
+
+void displayTelemetry()
+{
+    char text[90];
+    sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth);
+    pc.printf("%s", text);
+}
+
+//-----------End Helper functions--------------//
+
+
+
+//-----------Interface States------------------//
+enum controlStates { init, idle, manual, preparePid, beginTune } controlState;
+
+void controlInterface(){
+    
+    switch(controlState)            //Actions
+    {
+        case init:
+            pc.printf("Initialize sensors \n");
+            IMUinit(mpu1);
+            compass.init();
+            
+            pc.printf("Initialize motors  \n");
+            neutralizeMotors();
+            
+            pc.printf("Initialize PID objects \n");
+            initializePIDs();
+            
+            pc.printf("Calibrate MARG Filter \n");
+            calibrateFilter();
+        break;
+        
+        case idle:
+            IMUPrintData(mpu1, compass);
+            readDepth();
+            displayTelemetry();
+        break;
+        
+        case manual:
+            //pc.printf("Manual Control \n");
+            switch(command)
+            {
+                case 'N':
+                    //pc.printf("Neutralize motors\n");
+                    neutralizeMotors();
+                break;
+                
+                case 'R':
+                    //pc.printf("Reduce Thrust\n");
+                    if (m1pwm < pwmMin)
+                    {
+                        m1pwm++;   
+                        m2pwm++;
+                        m3pwm++;
+                        m4pwm++;
+                    }
+                break;
+                
+                case 'I':
+                    //pc.printf("Increase Thrust\n");
+                    if (m1pwm > pwmMax)
+                    {
+                        m1pwm--; 
+                        m2pwm--;
+                        m3pwm--;
+                        m4pwm--;
+                    }
+                break;
+                
+                default:
+                break;
+            }
+            updateMotors();
+        break;
+        
+        case preparePid:
+           // pc.printf("Prepare PID \n");
+            switch(command)
+            {
+                case 'I':           // Increase Kp gain
+                    kpVal += .1;
+                break;
+                
+                case 'R':
+                    if (kpVal > 0 ) // Decreae Kp gain
+                    {
+                        kpVal -= .1;  
+                    }
+                break;
+                
+                case 'S':           // Increase depth
+                    depthPoint++;
+                
+                case 's':           // Decrease setpoint
+                    if(depthPoint > 0)
+                    {
+                        depthPoint--;   
+                    }
+                break;
+                
+                default:
+                break;
+            }
+            
+            // Set tunings
+            pidd.SetTunings(kpVal,0,0);     //Set Ki Kd = 0 
+            
+            // Print Set parameters
+            pc.printf("DepthPoint: %d \n", depthPoint);
+            pc.printf("Kp gain: %d \n", pidd.GetKp());
+            
+        break;
+        
+        case beginTune:
+            //pc.printf("Tuning process begins.. \n");
+            
+            // Sense inertial / depth data
+            IMUPrintData(mpu1, compass);
+            readDepth();
+            
+            // Assign feedback variables
+            myDepth = depth;
+            
+            // Compute PID
+            pidd.Compute();
+            
+            // Output pwm to motors                     // FIXME : account for pitch, roll and maybe yaw depending on motor configuration
+            m1pwm = m2pwm = m3pwm = m4pwm = depthOut;
+            updateMotors();
+            
+            // Display telemetry
+            displayTelemetry();
+            
+        break;
+        
+        default:
+            pc.printf("Error! \n");
+        break;
+    }
+    
+    switch(controlState)            //Transitions
+    {
+        case init:
+            controlState = idle;
+        break;
+        
+        case idle:
+            controlState = (command == 'm') ? manual : ((command == 'p') ? preparePid : idle);
+        break;
+        
+        case manual:
+            controlState = (command == 'p') ? preparePid : manual;
+            if (controlState == preparePid)
+            {
+                //pc.printf("Neutralize Motors \n"); 
+                neutralizeMotors();  
+            }
+        break;
+        
+        case preparePid:
+            controlState = (command == 'g') ? beginTune : preparePid;
+        break;
+        
+        case beginTune:
+            controlState = (command == 't') ? preparePid : beginTune;
+            if (controlState == preparePid)
+            {
+                //pc.printf("Neutralize Motors \n");
+                neutralizeMotors();
+            }
+        break;
+        
+        default:
+        break;
+    }
+}
+
+//-----------End Interface---------------------//
+
+int main() {
+    // Initialize control state
+    controlState = init;
+    
+    while(1)
+    {
+        // Read user input
+        readUserInput();
+        
+        // Begin Interface
+        controlInterface();
+    }
+}
\ No newline at end of file