Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

TaskMove.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

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