Revision 10:d9f1037f0cb0, committed 2014-01-31

Comitter:

rkk

Date:

Fri Jan 31 05:18:19 2014 +0000

Parent:

9:8dd7a76756e2

Child:

11:8ec915eb70f6

Commit message:

still working on driving mode

Changed in this revision

--- a/main.cpp	Fri Jan 31 04:53:10 2014 +0000
+++ b/main.cpp	Fri Jan 31 05:18:19 2014 +0000
@@ -39,13 +39,12 @@
         
         float vol_norm=ch6.dutycyclescaledup();
         float freq_norm=ch3.dutycyclescaledup();
-        
-        
-        pc.printf("channel 3: %f, channel 6: %f\n", vol_norm, freq_norm);
+           
 
         //mcon.drive_rectangular(t.read(), vol_norm*freq_norm, 3*freq_norm);
         //mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm);
-        mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm);
+        float dutycycle = mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, freq_norm);
+        pc.printf("ch 3: %f, ch 6: %f, dc: %f, asin: %f\n", vol_norm, freq_norm, dutycycle, asin(sin(t.read())) );
         //pc.printf("time: %f\n\r", t.read());
         //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder);
         wait_ms(20);

--- a/motor_controller.cpp	Fri Jan 31 04:53:10 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 05:18:19 2014 +0000
@@ -21,6 +21,7 @@
     ome1 = 0.0;
     amp1 = 0.0;
     phi1 = 0.0;
+    firstTime = true;
 }
 
 PololuMController::~PololuMController()
@@ -60,29 +61,41 @@
     return;
 }
 
-void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
+float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
 {
-   float ome2 = 2.0* MATH_PI * frequency;
-   float divisor = (ome2*currentTime);
-   float dutycycle;
-   float phi2;
-   if(divisor > 0.0 && amplitude > 0.0)
-   {
-   phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
-   dutycycle = amplitude*sin(ome2 * currentTime + phi2);
-   phi1 = phi2;
-   }
-   else{
-   dutycycle = 0.0;
-   }
-   setpolarspeed(dutycycle);
-   
-   //set previous values
-   ome1 = ome2;
-   
-   amp1 = amplitude;
-   
-   return;
+    
+    //convert inputs
+    float amp2;
+    if(amplitude > 1.0) {
+        amp2 = 1.0;
+    } else if(amplitude < 0.0) {
+        amp2 = 0.0;
+    } else {
+        amp2 = amplitude;
+    }
+    float ome2 = 2.0* MATH_PI * frequency;
+
+    float dutycycle;
+    float phi2;
+    if (firstTime)
+    {
+        dutycycle = amp2*sin(ome2 * currentTime);
+        firstTime = false;
+    }
+    else if(amp2 > 0.0) {
+        phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime);
+        dutycycle = amp2*sin(ome2 * currentTime + phi2);
+        phi1 = phi2;
+    } else {
+        dutycycle = 0.0;
+    }
+    setpolarspeed(dutycycle);
+
+    //set previous values
+    ome1 = ome2;
+    amp1 = amp2;
+
+    return dutycycle;
 }
 
 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)

--- a/motor_controller.h	Fri Jan 31 04:53:10 2014 +0000
+++ b/motor_controller.h	Fri Jan 31 05:18:19 2014 +0000
@@ -16,7 +16,7 @@
     
     //for driving
     float phi1, ome1,amp1;
-    
+    bool firstTime;
     
     public:
     PololuMController();
@@ -25,6 +25,6 @@
     void setspeed(float speed);         //0 to 1
     void setpolarspeed(float speed);    //-1 to 1
     void reverse();                     //only works on non-polar speed
-    void drive_sinusoidal(float currentTime, float amplitude, float frequency);
+    float drive_sinusoidal(float currentTime, float amplitude, float frequency);
     void drive_rectangular(float currentTime, float amplitude, float frequency);
 };
\ No newline at end of file