File content as of revision 11:8ec915eb70f6:

#include "motor_controller.h"
//
//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(pwmport),outA(A),outB(B){
    outA.write(0);
    outB.write(1);
//    timestamp=0;
//    ome1 = 0.0;
//    amp1 = 0.0;
//    phi1 = 0.0;
//    firstTime = true;

}


//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));
    }
    else
    {
        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;
//
//    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;
//}