Revision 19:655db88b045c, committed 2014-05-28

Comitter:

rkk

Date:

Wed May 28 01:48:23 2014 +0000

Parent:

18:9ba4566f2361

Child:

20:6ae16da1492a

Commit message:

added yaw controller with adjustable frequency for either half to do a turning motion

Changed in this revision

--- a/MainController.cpp	Thu May 22 13:35:01 2014 +0000
+++ b/MainController.cpp	Wed May 28 01:48:23 2014 +0000
@@ -13,14 +13,20 @@
      
 {
     wait_ms(50);
-    vol = 0.0;
+    amp = 0.0;
     frq = 1.0;
-    rud = 0.5;
+    frqCmd = frq;
+    yaw = 0.5;
     pitch = 0.5;
     frqMin = 0.4; // hardcoded
     frqMax = 2.5; //hardcoded
-    change = 0;
-    state = 0;
+    //change = 0;
+    //state = 0;
+    fullCycle = true;
+    volume = 0.0;
+    volMax = 0.1;
+    volChg = 0.0;
+    ampCmd = 0.0;
     //goofftime = 0.0;
     //switched = false;
 }
@@ -29,77 +35,97 @@
 {
     curTime = timer1.read();
     
-    if(curTime > 1/frq) {
-        //read new inputs
-        vol = this->calculateVolume();
-        frq = this->calculateFrequency();
-        rud = this->calculateRudder();
+    // check every half cycle
+    if(curTime > 1/(2*frqCmd) ) {
+        
+        // read new yaw value every half cycle
+        yaw = this->calculateYaw(); // a value from -1 to 1
+        
+        if(yaw < 0.075 && yaw > -0.075){
+            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 ) {
+            //read other new inputs
+            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;
+                }
+            }
+            
+//            if(yaw >0.0)
+//            {
+//                ampNew = amp + yaw*amp;
+//                ampNew = (ampNew > 1.0) ? 1.0 : ampNew;
+//                
+//            }
+  
+            fullCycle = false;
+
+        } else {
+            // 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;
+                }
+            }
+//            if(yaw < 0.0)
+//            {
+//                ampNew = amp - yaw*amp;
+//                ampNew = (ampNew < -1.0) ? -1.0 : ampNew;
+//                
+//            }
+            
+            // 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) ) );
+
+        // 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
+        // ampCmd = vol * ( 2* rud + 1.0); // varied from 1 to 5      
+        ampCmd = amp * 2.0; // scale it up
+        
+        //reset timer
         timer1.reset();
         curTime = 0.0;
-
-        // rudder value used to define the trapezoid shape
-        // amplitude = vol * ( 2* rud + 1.0); // varied from 1 to 5
-        amplitude = vol * 4.0;
-        
-        //reset change to zero
-        change = 0;
+    }
         
-        if(state == 1) {
-            if(rud < 0.75) {
-                change = -1;
-                state = 0;
-            }
-        } 
-        else if (state == -1) {
-            if(rud> 0.25) {
-                change = 1;
-                state = 0;
-            }
-        } 
-        else {
-            if(rud < 0.25) {
-                change = -1;
-                state = -1;
-            } else if(rud > 0.75) {
-                change = 1;
-                state = 1;
-            }
-        }
-        //switched = false;
-        //goofftime = (vol/(2*frq));    
-    }
-    
+      
     //Set Servo Values
     //pitch = this->calculatePitch();
     //leftservo = pitch;
     //rightservo = 1.0 - pitch;
-       
-//    if (curTime > 1/(2*frq) && (switched == false))
-//    {
-//        switched = true;
-//        amplitude = -1.0;
-//     }
-//        
-//    if(!switched)
-//    {
-//        if(curTime > goofftime )
-//        amplitude = 0.0;
-//    }
-//    else
-//    {
-//        if(curTime > (1/(2*frq)+goofftime))
-//        amplitude = 0.0;
-//    }
 
-    // saturate ensures the duty cycle does not exceed 1, 
-    
-    if((change == 1) && (curTime > 1/(2*frq))) {
-        dutyCycle = 0.0;
-    } else if((change == -1) && (curTime < 1/(2*frq))) {
-        dutyCycle = 0.0;
-    } else {
-        dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime ));
-    }
+    dutyCycle = saturate(ampCmd * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+            
     mcon.setpolarspeed(dutyCycle);
 }
 
@@ -108,14 +134,14 @@
     return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
 }
 
-float MainController::calculateVolume()
+float MainController::calculateAmplitude()
 {
     return (ch6.dutycyclescaledup());
 }
 
-float MainController::calculateRudder()
+float MainController::calculateYaw()
 {
-    return (ch4.dutycyclescaledup());
+    return (2.0*ch4.dutycyclescaledup() - 1.0); 
 }
 
 float MainController::calculatePitch()
@@ -149,7 +175,7 @@
 
 float MainController::getAmplitude()
 {
-    return amplitude;
+    return amp;
 }
 
 
@@ -160,12 +186,12 @@
 
 float MainController::getVolume()
 {
-    return vol;
+    return volume;
 }
 
-float MainController::getRudder()
+float MainController::getYaw()
 {
-    return rud;
+    return yaw;
 }
 
 float MainController::getPitch()
@@ -173,6 +199,11 @@
     return pitch;
 }
 
+float MainController::getTimeAdd()
+{
+    return timeAdd;
+}
+
 //signum function
 float MainController::signum(float input)
 {

--- a/MainController.h	Thu May 22 13:35:01 2014 +0000
+++ b/MainController.h	Wed May 28 01:48:23 2014 +0000
@@ -34,8 +34,9 @@
     float getFrequency();
     float getVolume();
     float getAmplitude();
-    float getRudder();
+    float getYaw();
     float getPitch();
+    float getTimeAdd();
     
     /** Stop the main controller
      *
@@ -47,8 +48,8 @@
 protected:        
     void control();
     float calculateFrequency();
-    float calculateVolume();
-    float calculateRudder();
+    float calculateAmplitude();
+    float calculateYaw();
     float calculatePitch();
     float signum(float input);
     float saturate(float input);
@@ -66,18 +67,25 @@
     
     Timer timer1;
     Ticker ticker1;
-    float vol;
+    float amp;
     float frq;
     float dutyCycle;
     float curTime;
     float frqMin;
     float frqMax;
-    float amplitude;
-    float rud;
+    float yaw;
     float pitch;
     
-    int state;
-    int change;
+    bool fullCycle;
+    float volume;
+    float volChg;
+    float volMax;
+    float frqCmd;
+    float timeAdd;
+    float ampNew;
+    float ampCmd;
+    //int state;
+    //int change;
     //float goofftime;
     //bool switched;
 };

--- a/main.cpp	Thu May 22 13:35:01 2014 +0000
+++ b/main.cpp	Wed May 28 01:48:23 2014 +0000
@@ -18,8 +18,8 @@
 
     while(true) {
   
-        pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, pit: %f, t: %f\n",
-         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), t.read());
+        pc.printf("frq: %f, vol: %f, amp: %f, yaw: %f, dut: %f, pit: %f, tadd: %f, t: %f\n",
+         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(),mainCtrl.getTimeAdd(), t.read());
 
         wait_ms(100);
     }