File content as of revision 39:a4fd6206da89:

#include "RobotControl.h"

using namespace std;

RobotControl::RobotControl(MaxonESCON* motorControllerLeft,
                           MaxonESCON* motorControllerRight,
                           float period) : Task(period)
{
    /* get peripherals */
    this->motorControllerLeft = motorControllerLeft;
    this->motorControllerRight = motorControllerRight;
    this->period = period;

    /* initialize peripherals */
    motorControllerLeft->enable(false);
    motorControllerRight->enable(false);

    /* initialize remaining state values */
    speed = 0.0f;
    omega = 0.0f;

    motorControllerLeft->setPulses(0);
    motorControllerRight->setPulses(0);

    motorControllerLeft->setVelocity(0.0f);
    motorControllerRight->setVelocity(0.0f);
}

RobotControl::~RobotControl()
{

}

float RobotControl::PiRange(float theta)
{
    if(theta <= -PI) {
        return theta  += 2*PI;
    } else if (theta > PI) {
        return theta  -= 2*PI;
    } else {
        return theta;
    }
}

void RobotControl::setEnable(bool enable)
{
    motorControllerLeft->enable(enable);
    motorControllerRight->enable(enable);
}

bool RobotControl::isEnabled()
{
    return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
}

void RobotControl::setDesiredSpeed(float speed)
{
    this->speed = speed;
}

void RobotControl::setDesiredOmega(float omega)
{
    this->omega = omega;
}

void RobotControl::setDesiredxPosition(float xposition)
{
    Desired.xposition = xposition;
}

void RobotControl::setDesiredyPosition(float yposition)
{
    Desired.yposition = yposition;
}

void RobotControl::setDesiredTheta(float theta)
{
    Desired.theta = theta;
}

float RobotControl::getDesiredxPosition()
{
    return Desired.xposition;
}

float RobotControl::getDesiredyPosition()
{
    return Desired.yposition;
}

float RobotControl::getDesiredTheta()
{
    return Desired.theta;
}

void RobotControl::setDesiredPositionAndAngle(float xposition,
        float yposition,
        float theta)
{
    setDesiredxPosition(xposition);
    setDesiredyPosition(yposition);
    setDesiredTheta(theta);
}

float RobotControl::getTheta()
{
    return Desired.theta;
}

float RobotControl::getDesiredSpeed()
{
    return speed;
}

float RobotControl::getActualSpeed()
{
    return Actual.speed;
}

float RobotControl::getDesiredOmega()
{
    return omega;
}

float RobotControl::getActualOmega()
{
    return Actual.omega;
}

float RobotControl::getxActualPosition()
{
    return Actual.getxPosition();
}

float RobotControl::getxPositionError()
{
    return Desired.getxPosition()-Actual.getxPosition();
}

float RobotControl::getyActualPosition()
{
    return Actual.getyPosition();
}

float RobotControl::getyPositionError()
{
    return Desired.getyPosition()-Actual.getyPosition();
}

float RobotControl::getActualTheta()
{
    return Actual.getTheta();
}

float RobotControl::getThetaError()
{
    return Desired.getTheta()-Actual.getTheta();
}

float RobotControl::getDistanceError()
{
    return sqrt( ( getxPositionError() * getxPositionError() ) + ( getyPositionError() * getyPositionError() ) );
}

float RobotControl::getThetaErrorToGoal()
{
    return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
}

float RobotControl::getThetaGoal()
{
    return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
}

void RobotControl::setAllToZero(float xZeroPos,
                                float yZeroPos,
                                float theta)
{
    Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f);
    Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
    stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
    stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
    speed = 0.0f;
    omega = 0.0f;
}

void RobotControl::run()
{
    // When the Motor is note enable set the desired speed to the acutal speed.
    // only used by setting the speed and omega via the WII control.
    if (!isEnabled()) {
        speed = 0.0f;
        omega = 0.0f;
        Desired.setState(&Actual);
    }

    // position calculation
    // Set the state of speed from Left und Right Wheel
    stateLeft.speed = motorControllerLeft->getActualSpeed() *
                      2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
    stateRight.speed = - motorControllerRight->getActualSpeed() *
                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ;

    //translational speed of the Robot (average)
    Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f;

    //rotational speed of the Robot
    Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE;

    //rotational theta of the Robot integrate the omega with the time
    Actual.theta += Actual.omega * period;
    Actual.theta = PiRange(Actual.theta);

    //translational X and Y Position. integrate the speed with the time
    Actual.xposition += (Actual.speed * period * cos(Actual.theta));
    Actual.yposition += (Actual.speed * period * sin(Actual.theta));

    //motor control
    if ( /*isEnabled() && */ ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {
        
        motorControllerLeft->enable(true);
        motorControllerRight->enable(true);
        //postition control
        speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
        omega = K2 * getThetaErrorToGoal() +
                K1 *
                (
                    (
                        sin(getThetaErrorToGoal() ) * cos(getThetaErrorToGoal() )
                    ) / getThetaErrorToGoal()
                ) *
                ( getThetaErrorToGoal()
                  + K3 * getThetaGoal() );

        motorControllerLeft->setVelocity(
            ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) /
            (2 * WHEEL_RADIUS_LEFT * PI * GEAR)
        );
        motorControllerRight->setVelocity(
            -( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) /
            (2 * WHEEL_RADIUS_RIGHT * PI * GEAR)
        );

    } else {
        
        motorControllerLeft->enable(false);
        motorControllerRight->enable(false);
        motorControllerLeft->setVelocity(0.0f);
        motorControllerRight->setVelocity(0.0f);
    }
}