File content as of revision 1:aed2cca33061:

#include "position_controller.h"

PositionController::PositionController(int accDist, float maxV, int targetX, int targetY) :
    accelerationDistance(accDist),
    maxVelocity(maxV),
    targetDistance(hypot(targetX, targetY)),
    targetDistanceX(targetX),
    targetDistanceY(targetY),
    targetRadians(atan2(targetY, targetX))
{
    if(targetDistance > accelerationDistance * 2.0) {
        maxJerk = (maxVelocity / 2.0) / (pow(accelerationDistance / 2.0, 2));
    } else {
        maxJerk = (maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0))/2.0) / (pow(targetDistance / 4.0, 2));
    }
}

float PositionController::generateVelocity(int position)
{
    float vel;
    if(targetDistance > accelerationDistance * 2.0) {
        if(position >= 0 && position < accelerationDistance / 2.0) {
            //acc acc
            vel = pow(position, 2) * maxJerk;
        } else if(position >= accelerationDistance / 2.0 && position < accelerationDistance) {
            //acc dec
            vel = pow(position - accelerationDistance, 2) * (-maxJerk) + maxVelocity;
        } else if(position >= accelerationDistance && position < targetDistance - accelerationDistance) {
            //max velocity
            vel = maxVelocity;
        } else if(position >= targetDistance - accelerationDistance && position < targetDistance - accelerationDistance / 2.0) {
            //dec acc
            vel = pow(position - (targetDistance - accelerationDistance), 2) * (-maxJerk) + maxVelocity;
        } else if(position >= targetDistance - accelerationDistance / 2.0){
            //dec dec
            vel = pow(position - targetDistance, 2) * maxJerk;
        }
    } else {
        if(position >= 0 && position < targetDistance / 4.0) {
            vel = pow(position, 2) * maxJerk;
        } else if(position >= targetDistance / 4.0 && position < targetDistance / 2.0) {
            //acc dec
            vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
        } else if(position >= targetDistance / 2.0 && position < targetDistance * (3.0/4.0)) {
            //dec acc
            vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
        } else if(position >= targetDistance * (3.0 / 4.0)){
            //dec dec
            vel = pow(position - targetDistance, 2) * maxJerk;
        }
    }

    return vel;
}

void PositionController::compute(int positionX, int positionY)
{
    velocity[0] = generateVelocity(((double)positionX / (double)targetDistanceX) * targetDistance) * cos(targetRadians);
    velocity[1] = generateVelocity(((double)positionY / (double)targetDistanceY) * targetDistance) * sin(targetRadians);
}

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

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