jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

Diff: motor_controller.cpp

Revision:
9:8dd7a76756e2
Parent:
8:0574a5db1fc4
Child:
10:d9f1037f0cb0
--- a/motor_controller.cpp	Fri Jan 31 04:33:44 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 04:53:10 2014 +0000
@@ -64,9 +64,24 @@
 {
    float ome2 = 2.0* MATH_PI * frequency;
    float divisor = (ome2*currentTime);
-   float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
+   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);
    
-   setpolarspeed(amplitude*sin(ome2 * currentTime + phi2));
+   //set previous values
+   ome1 = ome2;
+   
+   amp1 = amplitude;
+   
    return;
 }