File content as of revision 8:0574a5db1fc4:

#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=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;
}

PololuMController::~PololuMController()
{
    delete pwm;
    delete outA;
    delete outB;
}

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;
}

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(amplitude*sin(ome2 * currentTime + phi2));
   return;
}

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;
}