File content as of revision 3:65ae32169b33:

#include "position_controller.h"

PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
    accDistance(accDistance_),
    decDistance(decDistance_),
    initialVelocity(initialVelocity_),
    terminalVelocity(terminalVelocity_),
    maxVelocity(maxVelocity_)
{
}

void PositionController::targetXY(int targetX_, int targetY_)
{
    targetX = targetX_;
    targetY = targetY_;
    target = hypot((double)targetX, (double)targetY);
    radians = atan2((double)targetY, (double)targetX);
    if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
        enoughDistance = true;
    } else {
        enoughDistance = false;
        maxVelocity = initialVelocity + (maxVelocity - initialVelocity) * (target/(accDistance+decDistance));
        accTrue = accDistance * (target/(accDistance+decDistance));
        decTrue = decDistance * (target/(accDistance+decDistance));
    }
}

double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
{
    return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
}

double PositionController::getVelocity(int position)
{
    double vel = 0;
    if(enoughDistance) {
        if(position < 0) {
            vel = initialVelocity;
        } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
            vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
        } else if(position < target - (decDistance * fabs(maxVelocity - terminalVelocity))) {
            vel = maxVelocity;
        } else if(position < target) {
            vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decDistance * fabs(maxVelocity - terminalVelocity), decDistance * fabs(maxVelocity - terminalVelocity));
        } else if(position < 2 * target) {
            vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
        } else {
            vel = -maxVelocity;
        }
    } else {
        if(position < 0) {
            vel = initialVelocity;
        } else if(position < accTrue) {
            vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accTrue);
        } else if(position < target) {
            vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decTrue, decTrue);
        } else if(position < 2 * target) {
            vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
        } else {
            vel = -maxVelocity;
        }
    }
    return vel;
}

void PositionController::compute(int positionX, int positionY)
{
    velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
    velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
}

double PositionController::getVelocityX()
{
    return velocity[0];
}

double PositionController::getVelocityY()
{
    return velocity[1];
}