File content as of revision 0:31f7be68e52d:

#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 acc)
{
    Desired.setAcceleration(acc);

}

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

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

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

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

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

void RobotControl::setTheta(float theta)
{
    Desired.theta = 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();
}

void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
{
    Actual.setState(xZeroPos, yZeroPos, 0.0f, 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()
{
    /* motion planning */
    if (isEnabled()) {
        Desired.increment(speed, omega, period);
    } 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;
    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;

    /* 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 */
    Actual.theta += Actual.omega * period;
    if(Actual.theta <= -PI) {
        Actual.theta  += 2*  PI;
    } else if (Actual.theta > PI) {
        Actual.theta  -= 2*  PI;
    } else {
        //nothing
    }
    Actual.theta += Actual.omega * period;
    Actual.thetaCompass = compass->getFilteredAngle();

       /* translational X and Y Position. integrate the speed with the time */ 
  //  Actual.xposition += (Actual.speed * period * sin(Actual.theta));
  //  Actual.yposition += (Actual.speed * period * cos(Actual.theta));
    
        /* translational X and Y Position. integrate the speed with the time theta from compass */
    Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass));
    Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass));
    
    
    
    
    /* postition control calculate */
    
    rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
    
    
    
    

    /* motor control */
    if (isEnabled()) {

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


    } else {

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

    }    
}