File content as of revision 4:4cc455e372c2:

#include "position_controller.h"

PositionController::PositionController(float accDistance_, float decDistance_, float initialVelocity_, float 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((float)targetX, (float)targetY);
    radians = atan2((float)targetY, (float)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));
    }
}

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

float PositionController::getVelocity(int position)
{
    float vel = 0;
    if(enoughDistance) {
        if(position < 0.0f) {
            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.0f * 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(((float)positionX / (float)targetX)*target) * cos(radians);
    velocity[1] = getVelocity(((float)positionY / (float)targetY)*target) * sin(radians);
}

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

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