Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

TaskMove.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

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