File content as of revision 5:48a258f6335e:

#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);

    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 temp;
    temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta();

    if(temp <= -PI) {
        temp  += 2*  PI;
    } else if (temp > PI) {
        temp  -= 2*  PI;
    } else {
        //nothing
    }
    return temp;*/
}

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

    /*
    if(temp <= -PI) {
        temp  += 2*  PI;
    } else if (temp > PI) {
        temp  -= 2*  PI;
    } else {
        //nothing
    }
    return temp;*/
}

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 * PI * GEAR;
    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * 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);
    /*
        if(Actual.theta <= -PI) {
            Actual.theta  += 2*  PI;
        } else if (Actual.theta > PI) {
            Actual.theta  -= 2*  PI;
        } else {
            //nothing
        }
    */
    /* 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 * PI * GEAR) );
        motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * 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;
    }
}