File content as of revision 1:08ca9b208045:

/*
 * TaskMoveTo.cpp
 * Copyright (c) 2017, ZHAW
 * All rights reserved.
 */

#include <cmath>
#include "TaskMoveTo.h"

using namespace std;

const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f;
const float TaskMoveTo::DEFAULT_ZONE = 0.01f;
const float TaskMoveTo::PI = 3.14159265f;
const float TaskMoveTo::K1 = 2.0f;
const float TaskMoveTo::K2 = 1.0f;
const float TaskMoveTo::K3 = 0.5f;

/**
 * Creates a task object that moves the robot to a given pose.
 * @param conroller a reference to the controller object of the robot.
 * @param x the x coordinate of the target position, given in [m].
 * @param y the y coordinate of the target position, given in [m].
 * @param alpha the target orientation, given in [rad].
 */
TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
    
    this->x = x;
    this->y = y;
    this->alpha = alpha;
    this->velocity = DEFAULT_VELOCITY;
    this->zone = DEFAULT_ZONE;
}

/**
 * Creates a task object that moves the robot to a given pose.
 * @param conroller a reference to the controller object of the robot.
 * @param x the x coordinate of the target position, given in [m].
 * @param y the y coordinate of the target position, given in [m].
 * @param alpha the target orientation, given in [rad].
 * @param velocity the maximum translational velocity, given in [m/s].
 */
TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
    
    this->x = x;
    this->y = y;
    this->alpha = alpha;
    this->velocity = velocity;
    this->zone = DEFAULT_ZONE;
}

/**
 * Creates a task object that moves the robot to a given pose.
 * @param conroller a reference to the controller object of the robot.
 * @param x the x coordinate of the target position, given in [m].
 * @param y the y coordinate of the target position, given in [m].
 * @param alpha the target orientation, given in [rad].
 * @param velocity the maximum translational velocity, given in [m/s].
 * @param zone the zone threshold around the target position, given in [m].
 */
TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
    
    this->x = x;
    this->y = y;
    this->alpha = alpha;
    this->velocity = velocity;
    this->zone = zone;
}

/**
 * Deletes the task object.
 */
TaskMoveTo::~TaskMoveTo() {}

/**
 * This method is called periodically by a task sequencer.
 * @param period the period of the task sequencer, given in [s].
 * @return the status of this task, i.e. RUNNING or DONE.
 */
int TaskMoveTo::run(float period) {
    
    // bitte implementieren!
    
    float rho  = sqrt(pow(this->x - this->controller.getX(),2.0f) + pow(this->y - this->controller.getY(),2.0f));
    if (rho == 0.0f) { // no need to move, already in range
    }
    else {
        float gama = atan2(this->y - this->controller.getY(),this->x - this->controller.getX()) - this->alpha;
        if (gama == 0) {
            this->controller.setTranslationalVelocity(this->K1 * rho * cos(gama));
        }
        else if (gama > this->PI) {
            gama = gama - PI;
        }
        else if (gama < this->PI) {
            gama = gama + PI;
        }
        else {
            float delta = gama + this->alpha - this->controller.getAlpha();
            if (delta > this->PI) {
                delta = delta - PI;
            }
            else if (delta < this->PI) {
                delta = delta + PI;
            }
            this->controller.setTranslationalVelocity(this->K1 * rho * cos(gama));
            this->controller.setRotationalVelocity(this->K2*gama+this->K1*sin(gama)*cos(gama)*(gama+this->K3*delta)/gama);
        }
    }
            
    if (rho < zone)
        return DONE;
    else 
        return RUNNING;

}