File content as of revision 0:1a0321f1ffbc:

/*
 * TaskMove.cpp
 * Copyright (c) 2018, ZHAW
 * All rights reserved.
 */

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

using namespace std;

const float TaskMove::DEFAULT_DURATION = 3600.0f;

/**
 * Creates a task object that moves the robot with given velocities.
 * @param conroller a reference to the controller object of the robot.
 * @param translationalVelocity the translational velocity, given in [m/s].
 * @param rotationalVelocity the rotational velocity, given in [rad/s].
 */
TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity) : controller(controller) {
    
    this->translationalVelocity = translationalVelocity;
    this->rotationalVelocity = rotationalVelocity;
    this->duration = DEFAULT_DURATION;
    
    time = 0.0f;
}

/**
 * Creates a task object that moves the robot with given velocities.
 * @param conroller a reference to the controller object of the robot.
 * @param translationalVelocity the translational velocity, given in [m/s].
 * @param rotationalVelocity the rotational velocity, given in [rad/s].
 * @param duration the duration to move the robot, given in [s].
 */
TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration) : controller(controller) {
    
    this->translationalVelocity = translationalVelocity;
    this->rotationalVelocity = rotationalVelocity;
    this->duration = duration;
    
    time = 0.0f;
}

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

/**
 * 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 TaskMove::run(float period) {
    
    time += period;
    
    if (time < duration) {
        
        controller.setTranslationalVelocity(translationalVelocity);
        controller.setRotationalVelocity(rotationalVelocity);
        
        return RUNNING;
        
    } else {
        
        controller.setTranslationalVelocity(0.0f);
        controller.setRotationalVelocity(0.0f);
        
        return DONE;
    }
}