File content as of revision 8:696c2f9dfc62:

#include "RobotControl.h"

using namespace std;

RobotControl::RobotControl(MaxonESCON* motorControllerLeft,
                           MaxonESCON* motorControllerRight,
                           /*HMC6352* compass,*/
                           float period) : Task(period)
{
    /* get peripherals */
    this->motorControllerLeft = motorControllerLeft;
    this->motorControllerRight = motorControllerRight;
    //  this->compass = compass;
    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);

    Desired.setAcceleration(ACCELERATION);
    Desired.setThetaAcceleration(THETA_ACCELERATION);
}

RobotControl::~RobotControl()
{

}

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

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

void RobotControl::setAcceleration(float acceleration)
{
    Desired.setAcceleration(acceleration);
}

void RobotControl::setThetaAcceleration(float acceleration)
{
    Desired.setThetaAcceleration(acceleration);
}

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()
{
    ///// DAs kan glaub raus ab hier
/////////////////////////////////////////////////////////7
    /* motion planning */
    if (isEnabled()) {
        ///// DAs kan glaub raus bis hier
        Desired.increment(speed, omega, period);

        ///// DAs kan glaub raus bis hier

    } else {
        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));

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

    /* motor control */
    if ( isEnabled() &&  ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {

        /* 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->setVelocity(0.0f);
        motorControllerRight->setVelocity(0.0f);

    }
}

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