Revision 20:6ae16da1492a, committed 2014-05-30

Comitter:

rkk

Date:

Fri May 30 22:18:39 2014 +0000

Parent:

19:655db88b045c

Child:

21:835fd919b4bd

Commit message:

working setup for yaw motion

Changed in this revision

--- a/MainController.cpp	Wed May 28 01:48:23 2014 +0000
+++ b/MainController.cpp	Fri May 30 22:18:39 2014 +0000
@@ -1,15 +1,15 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch1(p18,0.054,0.092), // pitch
-     ch2(p30,0.054,0.092), // roll
-     ch3(p16,0.054,0.092), // frequency
-     ch4(p17,0.055,0.092), //rudder
-     ch6(p15,0.055,0.092), //volume
+    :ch1(p18,0.056,0.090), // yaw
+     ch2(p17,0.054,0.092), // pitch
+     ch3(p15,0.054,0.092), // amplitude
+     ch4(p30,0.055,0.092), // adj
+     ch6(p16,0.053,0.092), // frequency
      mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
-     ap(p25, p26)//,
-     //leftservo(p21),
-     //rightservo(p22)
+     //ap(p25, p26)//,
+     leftservo(p21),
+     rightservo(p23)
      
 {
     wait_ms(50);
@@ -18,17 +18,22 @@
     frqCmd = frq;
     yaw = 0.5;
     pitch = 0.5;
-    frqMin = 0.4; // hardcoded
-    frqMax = 2.5; //hardcoded
+    adj = 0.5;
+    frqMin = 0.8; //hardcoded
+    frqMax = 2.9; //hardcoded
     //change = 0;
     //state = 0;
     fullCycle = true;
     volume = 0.0;
     volMax = 0.1;
     volChg = 0.0;
-    ampCmd = 0.0;
-    //goofftime = 0.0;
-    //switched = false;
+    raiser = 0.0;
+    pitAvg = 0.0;
+    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
+    
+    //leftservo.calibrate(0.0008, 45); 
+    //rightservo.calibrate(-0.0001, 45); 
+
 }
 
 void MainController::control()
@@ -41,12 +46,12 @@
         // read new yaw value every half cycle
         yaw = this->calculateYaw(); // a value from -1 to 1
         
-        if(yaw < 0.075 && yaw > -0.075){
+        if(yaw < 0.1 && yaw > -0.1){
             yaw =0.0;
         }
         // calculate liquid volume change in the chambers
         volChg = volMax * yaw;
-        //volChg = 0.0;
+        volChg = 0.0;
         
         timeAdd = 0.0;
         
@@ -68,13 +73,12 @@
                     volChg = timeAdd * amp;
                 }
             }
+            ampNew = amp;
             
-//            if(yaw >0.0)
-//            {
-//                ampNew = amp + yaw*amp;
-//                ampNew = (ampNew > 1.0) ? 1.0 : ampNew;
-//                
-//            }
+            if(yaw < 0.0)
+            {
+                ampNew = (1.0+0.7*yaw)*amp;
+            }
   
             fullCycle = false;
 
@@ -94,12 +98,13 @@
                     volChg = timeAdd * amp;
                 }
             }
-//            if(yaw < 0.0)
-//            {
-//                ampNew = amp - yaw*amp;
-//                ampNew = (ampNew < -1.0) ? -1.0 : ampNew;
-//                
-//            }
+            
+            ampNew = amp;
+            
+            if(yaw > 0.0)
+            {
+                ampNew = (1.0-0.7*yaw)*amp;
+            }
             
             // use amp and frq from last cycle in order to make sure it is equalized
             fullCycle = true;
@@ -107,11 +112,13 @@
         // update the frequency that actually needs to be commanded
         frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* 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
-        // ampCmd = vol * ( 2* rud + 1.0); // varied from 1 to 5      
-        ampCmd = amp * 2.0; // scale it up
+        raiser = ( 5 * adj + 1.0); // varied from 1 to 5      
         
         //reset timer
         timer1.reset();
@@ -120,33 +127,42 @@
         
       
     //Set Servo Values
-    //pitch = this->calculatePitch();
-    //leftservo = pitch;
-    //rightservo = 1.0 - pitch;
-
-    dutyCycle = saturate(ampCmd * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+    pitch = this->calculatePitch();
+    leftservo = pitch+0.03;
+    rightservo = 1.0 - pitch;
+    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
             
     mcon.setpolarspeed(dutyCycle);
 }
 
 float MainController::calculateFrequency()
 {
-    return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
+    return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);    
 }
 
 float MainController::calculateAmplitude()
 {
-    return (ch6.dutycyclescaledup());
+    return (ch3.dutycyclescaledup());
 }
 
 float MainController::calculateYaw()
 {
-    return (2.0*ch4.dutycyclescaledup() - 1.0); 
+    return (2.0*ch1.dutycyclescaledup() - 1.0); 
 }
 
 float MainController::calculatePitch()
 {
-    return (ch1.dutycyclescaledup());
+    pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
+    return pitAvg;
+}
+
+float MainController::calculateAdj()
+{
+    return (ch4.dutycyclescaledup());
 }
 
 void MainController::start()
@@ -157,7 +173,7 @@
     //Autopilot guardian
     //ap.calibrate();
     //ap.set2D();
-    ap.setoff();
+    //ap.setoff();
     
 }
 
@@ -199,6 +215,11 @@
     return pitch;
 }
 
+float MainController::getAdj()
+{
+    return adj;
+}
+
 float MainController::getTimeAdd()
 {
     return timeAdd;
@@ -224,4 +245,4 @@
         return (-1.0);
     else
         return input;
-}
\ No newline at end of file
+}

--- a/MainController.h	Wed May 28 01:48:23 2014 +0000
+++ b/MainController.h	Fri May 30 22:18:39 2014 +0000
@@ -4,7 +4,7 @@
 #include "mbed.h"
 #include "PwmIn.h"
 #include "motor_controller.h"
-#include "guardian.h"
+//#include "guardian.h"
 //#include "IMU.h"
 #include "Servo.h"
 
@@ -37,7 +37,8 @@
     float getYaw();
     float getPitch();
     float getTimeAdd();
-    
+    float getAdj();
+
     /** Stop the main controller
      *
      * @returns 
@@ -51,6 +52,7 @@
     float calculateAmplitude();
     float calculateYaw();
     float calculatePitch();
+    float calculateAdj();
     float signum(float input);
     float saturate(float input);
     
@@ -61,13 +63,14 @@
     PwmIn ch4;
     PwmIn ch6;
     PololuMController mcon;
-    Guardian ap;
-    //Servo leftservo;
-    //Servo rightservo;
+    //Guardian ap;
+    Servo leftservo;
+    Servo rightservo;
     
     Timer timer1;
     Ticker ticker1;
     float amp;
+    float ampNew;
     float frq;
     float dutyCycle;
     float curTime;
@@ -75,6 +78,7 @@
     float frqMax;
     float yaw;
     float pitch;
+    float adj;
     
     bool fullCycle;
     float volume;
@@ -82,12 +86,9 @@
     float volMax;
     float frqCmd;
     float timeAdd;
-    float ampNew;
-    float ampCmd;
-    //int state;
-    //int change;
-    //float goofftime;
-    //bool switched;
+    float raiser;
+    float pitAvg;
+    float alPi;
 };
  
 #endif

--- a/main.cpp	Wed May 28 01:48:23 2014 +0000
+++ b/main.cpp	Fri May 30 22:18:39 2014 +0000
@@ -18,8 +18,8 @@
 
     while(true) {
   
-        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());
+        pc.printf("frq: %.2f, vol: %.2f, amp: %.2f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, tadd: %.2f, t: %.2f\n",
+         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), mainCtrl.getTimeAdd(), t.read());
 
         wait_ms(100);
     }