Diff: motor_controller.cpp

Revision:

11:8ec915eb70f6

Parent:

10:d9f1037f0cb0

Child:

12:7eeb29892625

--- a/motor_controller.cpp	Fri Jan 31 05:18:19 2014 +0000
+++ b/motor_controller.cpp	Fri Jan 31 19:36:28 2014 +0000
@@ -1,109 +1,101 @@
 #include "motor_controller.h"
-
-float sigm(float input)
-{
-    if (input>0)
-        return 1;
-    else if (input<0)
-        return -1;
-    else
-        return 0;
-}
+//
+//float sigm(float input)
+//{
+//    if (input>0)
+//        return 1;
+//    else if (input<0)
+//        return -1;
+//    else
+//        return 0;
+//}
 
 PololuMController::PololuMController(PinName pwmport, PinName A, PinName B)
-{
-    pwm=new PwmOut(pwmport);
-    outA=new DigitalOut(A);
-    outB=new DigitalOut(B);
-    outA->write(0);
-    outB->write(1);
-    timestamp=0;
-    ome1 = 0.0;
-    amp1 = 0.0;
-    phi1 = 0.0;
-    firstTime = true;
+:pwm(pwmport),outA(A),outB(B){
+    outA.write(0);
+    outB.write(1);
+//    timestamp=0;
+//    ome1 = 0.0;
+//    amp1 = 0.0;
+//    phi1 = 0.0;
+//    firstTime = true;
+
 }
 
-PololuMController::~PololuMController()
-{
-    delete pwm;
-    delete outA;
-    delete outB;
-}
 
-void PololuMController::setspeed(float speed)
-{
-    pwm->write(speed);
-    return;
-}
+//void PololuMController::setspeed(float speed)
+//{
+//    pwm->write(speed);
+//    return;
+//}
 
 void PololuMController::setpolarspeed(float speed)
 {
     if (speed>=0)
     {
-        outA->write(0);
-        outB->write(1);
-        pwm->write(abs(speed));
+        outA.write(0);
+        outB.write(1);
+        pwm.write(abs(speed));
     }
     else
     {
-        outA->write(1);
-        outB->write(0);
-        pwm->write(abs(speed));
+        outA.write(1);
+        outB.write(0);
+        pwm.write(abs(speed));
     }
     return;
 }
 
-void PololuMController::reverse()
-{
-    outA->write(!(outA->read()));
-    outB->write(!(outB->read()));
-    return;
-}
-
-float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
-{
-    
-    //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;
+//void PololuMController::reverse()
+//{
+//    outA->write(!(outA->read()));
+//    outB->write(!(outB->read()));
+//    return;
+//}
 
-    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)
-{    
-    //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
-    float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
-    
-    float dutycycle = amplitude * sigm( sinRes);
-    setpolarspeed(dutycycle);
-    return;
-}
+//float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
+//{
+//   
+//    //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)
+//{    
+//    //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
+//    float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
+//    
+//    float dutycycle = amplitude * sigm( sinRes);
+//    setpolarspeed(dutycycle);
+//    return;
+//}