Diff: motor_controller.cpp

Revision:

8:0574a5db1fc4

Parent:

7:e005cfaff8d1

Child:

9:8dd7a76756e2

--- a/motor_controller.cpp	Thu Jan 30 02:04:23 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 04:33:44 2014 +0000
@@ -18,6 +18,9 @@
     outA->write(0);
     outB->write(1);
     timestamp=0;
+    ome1 = 0.0;
+    amp1 = 0.0;
+    phi1 = 0.0;
 }
 
 PololuMController::~PololuMController()
@@ -57,11 +60,14 @@
     return;
 }
 
-void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency)
+void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
 {
+   float ome2 = 2.0* MATH_PI * frequency;
+   float divisor = (ome2*currentTime);
+   float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
    
-    setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* frequency * currentTime));
-    return;
+   setpolarspeed(amplitude*sin(ome2 * currentTime + phi2));
+   return;
 }
 
 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)