Revision 9:9aaa7f0c8960, committed 2016-07-30

Comitter:

gelmes

Date:

Sat Jul 30 17:19:21 2016 +0000

Parent:

8:2db98df6fbbb

Child:

10:8cd741a65646

Commit message:

Untested Switching Program;

Changed in this revision

--- a/communication.h	Fri Jul 29 16:44:24 2016 +0000
+++ b/communication.h	Sat Jul 30 17:19:21 2016 +0000
@@ -1,5 +1,6 @@
 #ifndef COMMUNICATION_H
 #define COMMUNICATION_H
 MODSERIAL pc(USBTX, USBRX); // tx, rx
+MODSERIAL device(PA_2, PA_3); // tx, rx
 
 #endif
\ No newline at end of file

--- a/main.cpp	Fri Jul 29 16:44:24 2016 +0000
+++ b/main.cpp	Sat Jul 30 17:19:21 2016 +0000
@@ -29,7 +29,7 @@
     while(1) {
         //seagoat.motorTest();
         seagoat.update();
-        seagoat.updateCommand();
+        //seagoat.updateCommand();
         wait(0.01);
     }
 }
\ No newline at end of file

--- a/vessel.h	Fri Jul 29 16:44:24 2016 +0000
+++ b/vessel.h	Sat Jul 30 17:19:21 2016 +0000
@@ -12,28 +12,6 @@
 
 MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
 
-/*
-            Cameras
-      FL ----- F ->--- FR
-      |        |       |
-      ˄        |       |
-      |        |       |
-      L        |       R
-      |        |       |
-      |        |       ˅
-      |        |       |
-      BL ---<- B ----- BR
-
-      0  ----- 1 ->--- 2
-      |        |       |
-      ˄        |       |
-      |        |       |
-      7        |       3
-      |        |       |
-      |        |       ˅
-      |        |       |
-      6  ---<- 5 ----- 4
-*/
 #define BUFFER_SIZE 255
 
 class Vessel
@@ -51,6 +29,9 @@
 
     AnalogIn powerPin;
     int motorState;
+    int runningState;
+    Timer runningTime;
+
 //    PwmOut m0;
 //    PwmOut m1;
 //    PwmOut m2;
@@ -63,9 +44,9 @@
     PwmOut led1;
     MPU6050 mpu6050;
 
-    double yawPoint, yawIn, yawOut;
-    double rollPoint, rollIn, rollOut;
-    double pitchPoint, pitchIn, pitchOut;
+    double yawPoint, yawIn, yawOut, lastyawPoint;
+    double rollPoint, rollIn, rollOut, lastrollPoint;
+    double pitchPoint, pitchIn, pitchOut, lastpitchPoint;
     double xPoint, xIn, xOut;
     double yPoint, yIn, yOut;
     double zPoint, zIn, zOut;
@@ -83,6 +64,7 @@
         IMUinit(mpu6050);
         pressure_sensor.MS5837Init();
         IMUPrintData(mpu6050);
+        runningTime.start();
     }
 
     //Initialise all of the vessels starting parameters
@@ -94,8 +76,7 @@
         pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
         pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT),
         pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
-        powerPin(A5)
-{
+        powerPin(A5) {
 
         pidy.SetMode(AUTOMATIC);  //Yaw PID
         pidy.SetOutputLimits(-255,255);
@@ -128,7 +109,7 @@
         m5 = 0.5;
         m6 = 0.5;
         m7 = 0.5;
-        
+
         motorState = 1;
 
         Start_IMU();
@@ -162,28 +143,6 @@
         pidd.SetTunings(Kp, Ki, Kd);
     }
 
-    //This is where the magic happens
-//    void motorTest(){
-//            pwmSweep(m0);
-//            pwmSweep(m1);
-//            pwmSweep(m2);
-//            pwmSweep(m3);
-//            pwmSweep(m4);
-//            pwmSweep(m5);
-//            pwmSweep(m6);
-//            pwmSweep(m7);
-//    }
-//
-//    void pwmSweep(PwmOut motor){
-//        for(float i = 0; i < 80; i++){
-//            motor = i/255;
-//            wait(0.002);
-//        }
-//   //     for(float i = 80; i >= 0; i--){
-////            motor = i/255;
-////            wait(0.002);
-////        }
-//    }
     void calibrate() {
         IMUUpdate(mpu6050);
         pc.printf("Calibrating...\n\r");
@@ -198,16 +157,24 @@
         //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){
-            initMotors(); 
-            motorState = 1;  
+        if(!motorState && powerPin.read() == 1) {
+            runningTime.stop();
+            initMotors();
+            motorState = 1;
+            runningState += 1;
+            if(runningState == 2) runningState = 0;
+
             pc.printf("Motors Detected");
-            
-             yawPoint = yaw;
-        }
-        else if(powerPin.read() != 1){
+
+            yawPoint = yaw;
+            if(runningState == 1) pitchPoint = pitch;
+            else pitchPoint = 0;
+            dPoint = depth;
+            runningTime.reset();
+            runningTime.start();
+        } else if(powerPin.read() != 1) {
             motorState = 0;
             neutralizeMotors();
         }
@@ -253,6 +220,8 @@
 
         //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
 
+        float forwardThrust = 100;
+
         //Values used in Dynamic Magnitude Calculations
         float accxs = xOut * xOut * abs(xOut) / xOut;
         float accys = yOut * yOut * abs(yOut) / yOut;
@@ -267,119 +236,109 @@
         float yy  = (abs(yOut) + abs(yawOut)) * 255;
         float xy  = (abs(xOut) + abs(yawOut)) * 255;
 
-//        float zpr = (zOut + pitchOut + rollOut) * 255;
-//        float yy  = (yOut + yawOut) * 255;
-//        float xy  = (xOut + yawOut) * 255;
-
-//        if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
-//        if (abs(yy)<255 && abs(yy)>=0) yy = 255;
-//        if (abs(xy)<255 && abs(xy)>=0) xy = 255;
-//        if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
-//        if (abs(yy)>-255 && abs(yy)<0) yy = -255;
-//        if (abs(xy)>-255 && abs(xy)<0) xy = -255;
-
         if (abs(zpr)<255) zpr = 255;
         if (abs(yy)<255) yy = 255;
         if (abs(xy)<255) xy = 255;
 
-        //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
-
-        //Spit out PID values
-
-//        m0 = abs((acczs + pitchs + rolls) / zpr);//
-//        m1 = abs((accys + yaws) / yy);
-//        m2 = abs((acczs + pitchs - rolls) / zpr);//
-//        m3 = abs((accxs + yaws) / xy);
-//        m4 = abs((acczs - pitchs - rolls) / zpr);//
-//        m5 = abs((accys + yaws) / yy);
-//        m6 = abs((acczs - pitchs + rolls) / zpr);//
-//        m7 = abs((accxs + yaws) / yy);
-
-        m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
-        m1 = (accys + yaws) / -yy + 0.5;
-        m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
-        m3 = (accxs + yaws) / xy + 0.5;
-        m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
-        m5 = (accys + yaws) / -yy + 0.5;
-        m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
-        m7 = (accxs + yaws) / -yy + 0.5;
-
-        //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(), acczs, yaws, pitchs, rolls, zpr, depths, (acczs + pitchs + rolls - depths) / zpr + 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));
-        // pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
-        //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
-    }
-
-    void updateCommand() {
-        char c = 0;
-        string command;
-        char buffer[BUFFER_SIZE] = {' '};
-        int buffer_iter = 0;
-        //pc.printf("Checking for command\n");
-
-        // Note: you need to actually read from the serial to clear the RX interrupt
-        if (pc.readable()) {
-            pc.printf("Found command\n");
-            while (pc.readable()) {
-                c = pc.getc();
-                pc.putc(c);
-                buffer[buffer_iter] = c;
-                buffer_iter++;
-            }
-            pc.printf("Command saved to buffer\n");
-            command = strtok (buffer," ,\n");
-
-            if (strcmp(command.c_str(), "a")) {
-                this->yawPoint = atof(strtok (NULL, " ,\n"));
-                this->pitchPoint = atof(strtok (NULL, " ,\n"));
-                this->rollPoint = atof(strtok (NULL, " ,\n"));
-                pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
-            } else if (strcmp(command.c_str(), "b")) {
-                this->xPoint = atof(strtok (NULL, " ,\n"));
-                this->yPoint = atof(strtok (NULL, " ,\n"));
-                this->zPoint = atof(strtok (NULL, " ,\n"));
-                pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
-            } else if (strcmp(command.c_str(), "c")) {
-                this->p_gain = atof(strtok (NULL, " ,\n"));
-                this->i_gain = atof(strtok (NULL, " ,\n"));
-                this->d_gain = atof(strtok (NULL, " ,\n"));
-
-                this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
-                pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
-
+        if(runningState == 0) {
+                if(runningTime < 1) {
+                    SetYawPID(1,0,0);
+                    SetRollPID(1,0,0);
+                    SetPitchPID(1,0,0);
+                    wait(1);
+                }
+                else{
+                    m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
+                    m1 = (accxs + yaws) / -xy + 0.5;
+                    m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+                    //m3 = (accys + yaws) / yy + 0.5;
+                    m3 = 0.7;
+                    m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+                    m5 = (accxs + yaws) / -xy + 0.5;
+                    m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
+                    //m7 = (-accys + yaws) / -yy + 0.5;
+                    m7 = 0.7;
+                }
+        } 
+        else if(runningState == 1) {
+                if(runningTime < 1) {
+                    SetYawPID(2,0,0.3);
+                    SetRollPID(2,0,0.3);
+                    SetPitchPID(2,0,0.3);
+                    wait(1);
+                } else if(runningTime < 27) {
+                    m1 = (accxs + yaws) / -xy + 0.5;
+                    m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+                    //m3 = (accys + yaws) / yy + 0.5;
+                    m3 = 0.7;
+                    m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+                    m5 = (accxs + yaws) / -xy + 0.5;
+                    m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
+                    //m7 = (-accys + yaws) / -yy + 0.5;
+                    m7 = 0.7;
+                } else if(runningTime < 5) {
+                    m1 = (accxs + yaws) / -xy + 0.5;
+                    m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+                    //m3 = (accys + yaws) / yy + 0.5;
+                    m3 = 0.7;
+                    m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+                    m5 = (accxs + yaws) / -xy + 0.5;
+                    m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
+                    //m7 = (-accys + yaws) / -yy + 0.5;
+                    m7 = 0.7;
+                }
             }
 
-            memset(buffer, ' ', sizeof(buffer));
-            buffer_iter = 0;
-            fflush(stdout);
+            //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));
+            //pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
+            //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
         }
-    }
+
+        void updateCommand() {
+            char a = 0;
+            char i = 0;
+            char buffer[10] = {' '};
+            if (device.readable()) {
+                while(a != 'd') {
+                    a = device.getc();
+                    if ((a >= '0' && a <='9') || a == '.') {
+                        buffer[i] = a;
+                        i++;
+                    }
+                }
+                depth = atof(buffer);
+                pc.printf("Depth: '%f'\n", depth);
+            }
+        }
+
+        void initMotors() {
 
-    void initMotors(){
-        
-        neutralizeMotors();
-        m0.calibrate();
-        m1.calibrate();
-        m2.calibrate();
-        m3.calibrate();
-        m4.calibrate();
-        m5.calibrate();
-        m6.calibrate();
-        m7.calibrate();
-    }
-    
-    void neutralizeMotors(){
-        m0 = 0.5;
-        m1 = 0.5;
-        m2 = 0.5;
-        m3 = 0.5;
-        m4 = 0.5;
-        m5 = 0.5;
-        m6 = 0.5;
-        m7 = 0.5;
-    }
+            neutralizeMotors();
+            wait(0.5);
+            m0.calibrate();
+            m1.calibrate();
+            m2.calibrate();
+            m3.calibrate();
+            m4.calibrate();
+            m5.calibrate();
+            m6.calibrate();
+            m7.calibrate();
+            wait(3);
+        }
 
-};
+        void neutralizeMotors() {
+            m0 = 0.5;
+            m1 = 0.5;
+            m2 = 0.5;
+            m3 = 0.5;
+            m4 = 0.5;
+            m5 = 0.5;
+            m6 = 0.5;
+            m7 = 0.5;
+        }
+
+    };
 
 #endif
\ No newline at end of file