Revision 17:6970aef8154b, committed 2014-05-22

Comitter:

rkk

Date:

Thu May 22 04:32:19 2014 +0000

Parent:

16:79cfe6201318

Child:

18:9ba4566f2361

Commit message:

jet fish with simple yaw motion and added channels for pwm in and pwm out

Changed in this revision

--- a/MainController.cpp	Mon Feb 17 14:04:09 2014 +0000
+++ b/MainController.cpp	Thu May 22 04:32:19 2014 +0000
@@ -1,20 +1,28 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch3(p16,0.054,0.092), // frequency
+    :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
-     mcon(p22, p6, p7), // change p5 to p7, because p5 is burned through
-     ap(p25, p26)
+     mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
+     ap(p25, p26),
+     leftservo(p21),
+     rightservo(p22)
+     
 {
     wait_ms(50);
     vol = 0.0;
-    frq = 10.0;
+    frq = 1.0;
     rud = 0.5;
+    pitch = 0.5;
     frqMin = 0.4; // hardcoded
     frqMax = 2.5; //hardcoded
-    goofftime = 0.0;
-    switched = false;
+    change = 0;
+    state = 0;
+    //goofftime = 0.0;
+    //switched = false;
 }
 
 void MainController::control()
@@ -25,18 +33,46 @@
         //read new inputs
         vol = this->calculateVolume();
         frq = this->calculateFrequency();
-        rud = this->calculateRudder(); //not used right now
+        rud = this->calculateRudder();
         timer1.reset();
         curTime = 0.0;
-        
+
         // rudder value used to define the trapezoid shape
-        amplitude = vol * ( 2* rud + 1.0);
-        //amplitude = vol * 3.0;
+        // amplitude = vol * ( 2* rud + 1.0); // varied from 1 to 5
+        amplitude = vol * 4.0;
+        
+        //reset change to zero
+        change = 0;
         
-        switched = false;
-        goofftime = (vol/(2*frq));
-            
+        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))
 //    {
@@ -55,12 +91,18 @@
 //        amplitude = 0.0;
 //    }
 
-    dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime ));
-
+    // 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 ));
+    }
     mcon.setpolarspeed(dutyCycle);
 }
 
-
 float MainController::calculateFrequency()
 {
     return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
@@ -76,9 +118,15 @@
     return (ch4.dutycyclescaledup());
 }
 
+float MainController::calculatePitch()
+{
+    return (ch1.dutycyclescaledup());
+}
+
 void MainController::start()
 {
     timer1.start();
+    
     ticker1.attach(this, &MainController::control,0.001);
     //Autopilot guardian
     //ap.calibrate();
@@ -120,6 +168,11 @@
     return rud;
 }
 
+float MainController::getPitch()
+{
+    return pitch;
+}
+
 //signum function
 float MainController::signum(float input)
 {

--- a/MainController.h	Mon Feb 17 14:04:09 2014 +0000
+++ b/MainController.h	Thu May 22 04:32:19 2014 +0000
@@ -6,6 +6,8 @@
 #include "motor_controller.h"
 #include "guardian.h"
 //#include "IMU.h"
+#include "Servo.h"
+
 
 
 #define MATH_PI 3.14159265359
@@ -33,6 +35,7 @@
     float getVolume();
     float getAmplitude();
     float getRudder();
+    float getPitch();
     
     /** Stop the main controller
      *
@@ -46,16 +49,20 @@
     float calculateFrequency();
     float calculateVolume();
     float calculateRudder();
+    float calculatePitch();
     float signum(float input);
     float saturate(float input);
     
 private:
+    PwmIn ch1;
+    PwmIn ch2;
     PwmIn ch3;
     PwmIn ch4;
     PwmIn ch6;
     PololuMController mcon;
     Guardian ap;
-    
+    Servo leftservo;
+    Servo rightservo;
     
     Timer timer1;
     Ticker ticker1;
@@ -67,8 +74,12 @@
     float frqMax;
     float amplitude;
     float rud;
-    float goofftime;
-    bool switched;
+    float pitch;
+    
+    int state;
+    int change;
+    //float goofftime;
+    //bool switched;
 };
  
 #endif

--- a/main.cpp	Mon Feb 17 14:04:09 2014 +0000
+++ b/main.cpp	Thu May 22 04:32:19 2014 +0000
@@ -18,8 +18,8 @@
 
     while(true) {
   
-        pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, t: %f\n",
-         mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(),t.read());
+        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());
 
         wait_ms(100);
     }