TeamSurface / Libs

Nim leo niiiim

TaskMove.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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