Revision 23:ef1be367726e, committed 2014-06-05

Comitter:

sandwich

Date:

Thu Jun 05 17:32:51 2014 +0000

Parent:

22:807d5467fbf6

Child:

24:9d75ed1462d6

Commit message:

-has tele-op and autonomous mode; -left slider now controls gain on pitch; -works with syren now; -uses rtos thread model for better multitasking

Changed in this revision

--- a/MainController.cpp	Thu Jun 05 00:13:02 2014 +0000
+++ b/MainController.cpp	Thu Jun 05 17:32:51 2014 +0000
@@ -29,7 +29,9 @@
     fullCycle = true;
     volume = 0.0;
     volMax = 0.1;
-    volChg = 0.0;
+
+    yawAdjVal = 0.7;
+
     raiser = 0.0;
     pitAvg = 0.0;
     alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
@@ -69,24 +71,18 @@
         // check every half cycle
         if(curTime > 1/(2*frqCmd) ) {
             // read new yaw value every half cycle
-            if (ch5.dutycyclescaledup()<=0.5)
+            if (ch5.dutycyclescaledup()<=0.5) {
                 yaw = this->calculateYaw(); // a value from -1 to 1
-            else {
+                if(yaw < 0.1 && yaw > -0.1)
+                    yaw =0.0;
+            } else {
                 //do proportional control on fish yaw
-                float gain=1/float(CENTER_X);
+                float gain=(1/(1+ampCmd))*frqCmd*(1/float(CENTER_X));
                 float error=pcam.getBestX()-CENTER_X; //calculate yaw difference
-                yaw=error*gain;
-                //printf("xlocation: %d -> %f\n", pcam.getBestX(), yaw);
+                yaw=error*gain+0.5f;
+                //printf("x: %d, ", pcam.getBestX());
             }
 
-            if(yaw < 0.1 && yaw > -0.1) {
-                yaw =0.0;
-            }
-            // calculate liquid volume change in the chambers
-            volChg = volMax * yaw;
-            volChg = 0.0;
-
-            timeAdd = 0.0;
 
             // Read volume and frequency only every full cycle
             if( fullCycle ) {
@@ -94,22 +90,10 @@
                 amp = this->calculateAmplitude(); // a value from 0 to 1
                 frq = this->calculateFrequency(); // a value from frqmin to frqmax
 
-                if(volChg > 0.0) {
-                    // adjust frequency to add additional volume
-                    if( amp < 0.0001 ) {
-                        amp = 0.0001;
-                    }
-                    timeAdd = volChg/amp;
-
-                    if( timeAdd > 0.5/frq ) {
-                        timeAdd = 0.5/frq;
-                        volChg = timeAdd * amp;
-                    }
-                }
-                ampNew = amp;
-
                 if(yaw < 0.0) {
-                    ampNew = (1.0+0.7*yaw)*amp;
+                    ampCmd = (1.0+ yawAdjVal * yaw)*amp;
+                } else {
+                    ampCmd = amp;
                 }
 
                 fullCycle = false;
@@ -118,38 +102,27 @@
                 // reverse for the downward slope
                 amp = -amp;
 
-                if(volChg < 0.0) {
-                    // adjust frequency to add additional volume
-                    if( amp > -0.0001 ) {
-                        amp = -0.0001;
-                    }
-                    timeAdd = volChg/amp;
 
-                    if( timeAdd > 0.5/frq ) {
-                        timeAdd = 0.5/frq;
-                        volChg = timeAdd * amp;
-                    }
-                }
-
-                ampNew = amp;
 
                 if(yaw > 0.0) {
-                    ampNew = (1.0-0.7*yaw)*amp;
+                    ampCmd = (1.0- yawAdjVal *yaw)*amp;
+                } else {
+                    ampCmd = amp;
                 }
 
                 // use amp and frq from last cycle in order to make sure it is equalized
                 fullCycle = true;
             }
+
             // update the frequency that actually needs to be commanded
-            frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
+            frqCmd = frq;
 
             // read new yaw value every half cycle
             adj = this->calculateAdj(); // a value from 0 to 1
 
-            // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
-            //volume = volChg + volume;
-            // rudder value used to define the trapezoid shape
-            raiser = ( 5 * adj + 1.0); // varied from 1 to 5
+
+            // adj value used to define the trapezoid shape
+            raiser = ( 5 * 0.7 + 1.0); // varied from 1 to 5 for now hard coded
 
             //reset timer
             timer1.reset();
@@ -162,18 +135,28 @@
             pitch = this->calculatePitch();
         else {
             //do a proportional control on the error since servos already give you position control
-            float gain=1/float(CENTER_Y); //try this out for now
+            float gain=(1/(1+abs(ampCmd)))*(1/frqCmd)*(1/float(CENTER_Y))*adj; //try this out for now
+            //float gain=(1/float(CENTER_Y))*adj;
             float error=pcam.getBestY()-CENTER_Y;
-            pitch=error*gain;
-            //printf("ylocation: %d -> %f\n", pcam.getBestY(), pitch);
+            pitch=error*gain+0.5;
+            //printf("error: %f\n", error);
+            //printf("y: %d\n", pcam.getBestY());
+            //printf("pitch: %f\n", pitch);
         }
+
+        //Adjusting the trim of the pitch angles
         leftservo = pitch+0.03;
+        if (leftservo > 1.0) {
+            leftservo = 1.0;
+        }
         rightservo = 1.0 - pitch;
-        if (rightservo<0.03) {
+        if (rightservo < 0.03) {
             rightservo = 0.03;
         }
 
-        dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+
+        dutyCycle = ampCmd * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+        //printf("dc: %f\n", dutyCycle);
 
         //now scale duty cycle from -1 to 1 -> 0 to 255
         dutyCycle=(255/2)*dutyCycle+127;
@@ -183,6 +166,7 @@
             dutyCycle=255.0;
 
         //mcon.setpolarspeed(dutyCycle);
+        //dutyCycle=100;
         syren.putc(int(dutyCycle));
         //syren.putc(int(0));
         trackMutex.unlock();
@@ -289,11 +273,6 @@
     return adj;
 }
 
-float MainController::getTimeAdd()
-{
-    return timeAdd;
-}
-
 bool MainController::getOpMode()
 {
     return ch5.dutycyclescaledup()>0.5;

--- a/MainController.h	Thu Jun 05 00:13:02 2014 +0000
+++ b/MainController.h	Thu Jun 05 17:32:51 2014 +0000
@@ -40,7 +40,6 @@
     float getAmplitude();
     float getYaw();
     float getPitch();
-    float getTimeAdd();
     float getAdj();
     bool getOpMode(); //returns true when in autonomous mode
 
@@ -86,7 +85,7 @@
     Thread trackerThread;
     static void trackerThreadStarter(void const *p);
     float amp;
-    float ampNew;
+    float ampCmd;
     float frq;
     float dutyCycle;
     float curTime;
@@ -95,13 +94,11 @@
     float yaw;
     float pitch;
     float adj;
-    
+    float yawAdjVal;
     bool fullCycle;
     float volume;
-    float volChg;
     float volMax;
     float frqCmd;
-    float timeAdd;
     float raiser;
     float pitAvg;
     float alPi;

--- a/main.cpp	Thu Jun 05 00:13:02 2014 +0000
+++ b/main.cpp	Thu Jun 05 17:32:51 2014 +0000
@@ -17,10 +17,10 @@
 
     //printf("started\n");
     while(true) {
-        pc.printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n",
-                  mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), mainCtrl.getTimeAdd(), t.read());
+        pc.printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, t: %.2f\n",
+                  mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), t.read());
 
-        wait_ms(100);
+        //wait_ms(100);
     }
     //t.stop();
     //mainCtrl.stop();

--- a/pixy_cam.lib	Thu Jun 05 00:13:02 2014 +0000
+++ b/pixy_cam.lib	Thu Jun 05 17:32:51 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/jetfishteam/code/pixy_cam/#f54759b26096
+http://mbed.org/teams/jetfishteam/code/pixy_cam/#4bf8d39fc5ce