ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

TaskMove.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Inaueadr 0:29be10cb0afc 1 /*
Inaueadr 0:29be10cb0afc 2 * TaskMove.cpp
Inaueadr 0:29be10cb0afc 3 * Copyright (c) 2018, ZHAW
Inaueadr 0:29be10cb0afc 4 * All rights reserved.
Inaueadr 0:29be10cb0afc 5 */
Inaueadr 0:29be10cb0afc 6
Inaueadr 0:29be10cb0afc 7 #include <cmath>
Inaueadr 0:29be10cb0afc 8 #include "TaskMove.h"
Inaueadr 0:29be10cb0afc 9
Inaueadr 0:29be10cb0afc 10 using namespace std;
Inaueadr 0:29be10cb0afc 11
Inaueadr 0:29be10cb0afc 12 const float TaskMove::DEFAULT_DURATION = 3600.0f;
Inaueadr 0:29be10cb0afc 13
Inaueadr 0:29be10cb0afc 14 /**
Inaueadr 0:29be10cb0afc 15 * Creates a task object that moves the robot with given velocities.
Inaueadr 0:29be10cb0afc 16 * @param conroller a reference to the controller object of the robot.
Inaueadr 0:29be10cb0afc 17 * @param translationalVelocity the translational velocity, given in [m/s].
Inaueadr 0:29be10cb0afc 18 * @param rotationalVelocity the rotational velocity, given in [rad/s].
Inaueadr 0:29be10cb0afc 19 */
Inaueadr 0:29be10cb0afc 20 TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity) : controller(controller) {
Inaueadr 0:29be10cb0afc 21
Inaueadr 0:29be10cb0afc 22 this->translationalVelocity = translationalVelocity;
Inaueadr 0:29be10cb0afc 23 this->rotationalVelocity = rotationalVelocity;
Inaueadr 0:29be10cb0afc 24 this->duration = DEFAULT_DURATION;
Inaueadr 0:29be10cb0afc 25
Inaueadr 0:29be10cb0afc 26 time = 0.0f;
Inaueadr 0:29be10cb0afc 27 }
Inaueadr 0:29be10cb0afc 28
Inaueadr 0:29be10cb0afc 29 /**
Inaueadr 0:29be10cb0afc 30 * Creates a task object that moves the robot with given velocities.
Inaueadr 0:29be10cb0afc 31 * @param conroller a reference to the controller object of the robot.
Inaueadr 0:29be10cb0afc 32 * @param translationalVelocity the translational velocity, given in [m/s].
Inaueadr 0:29be10cb0afc 33 * @param rotationalVelocity the rotational velocity, given in [rad/s].
Inaueadr 0:29be10cb0afc 34 * @param duration the duration to move the robot, given in [s].
Inaueadr 0:29be10cb0afc 35 */
Inaueadr 0:29be10cb0afc 36 TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration) : controller(controller) {
Inaueadr 0:29be10cb0afc 37
Inaueadr 0:29be10cb0afc 38 this->translationalVelocity = translationalVelocity;
Inaueadr 0:29be10cb0afc 39 this->rotationalVelocity = rotationalVelocity;
Inaueadr 0:29be10cb0afc 40 this->duration = duration;
Inaueadr 0:29be10cb0afc 41
Inaueadr 0:29be10cb0afc 42 time = 0.0f;
Inaueadr 0:29be10cb0afc 43 }
Inaueadr 0:29be10cb0afc 44
Inaueadr 0:29be10cb0afc 45 /**
Inaueadr 0:29be10cb0afc 46 * Deletes the task object.
Inaueadr 0:29be10cb0afc 47 */
Inaueadr 0:29be10cb0afc 48 TaskMove::~TaskMove() {}
Inaueadr 0:29be10cb0afc 49
Inaueadr 0:29be10cb0afc 50 /**
Inaueadr 0:29be10cb0afc 51 * This method is called periodically by a task sequencer.
Inaueadr 0:29be10cb0afc 52 * @param period the period of the task sequencer, given in [s].
Inaueadr 0:29be10cb0afc 53 * @return the status of this task, i.e. RUNNING or DONE.
Inaueadr 0:29be10cb0afc 54 */
Inaueadr 0:29be10cb0afc 55 int TaskMove::run(float period) {
Inaueadr 0:29be10cb0afc 56
Inaueadr 0:29be10cb0afc 57 time += period;
Inaueadr 0:29be10cb0afc 58
Inaueadr 0:29be10cb0afc 59 if (time < duration) {
Inaueadr 0:29be10cb0afc 60
Inaueadr 0:29be10cb0afc 61 controller.setTranslationalVelocity(translationalVelocity);
Inaueadr 0:29be10cb0afc 62 controller.setRotationalVelocity(rotationalVelocity);
Inaueadr 0:29be10cb0afc 63
Inaueadr 0:29be10cb0afc 64 return RUNNING;
Inaueadr 0:29be10cb0afc 65
Inaueadr 0:29be10cb0afc 66 } else {
Inaueadr 0:29be10cb0afc 67
Inaueadr 0:29be10cb0afc 68 controller.setTranslationalVelocity(0.0f);
Inaueadr 0:29be10cb0afc 69 controller.setRotationalVelocity(0.0f);
Inaueadr 0:29be10cb0afc 70
Inaueadr 0:29be10cb0afc 71 return DONE;
Inaueadr 0:29be10cb0afc 72 }
Inaueadr 0:29be10cb0afc 73 }
Inaueadr 0:29be10cb0afc 74