Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

TaskMoveToWaypoint.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 * TaskMoveToWaypoint.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 "TaskMoveToWaypoint.h"
Brignall 0:1a0321f1ffbc 9
Brignall 0:1a0321f1ffbc 10 using namespace std;
Brignall 0:1a0321f1ffbc 11
Brignall 0:1a0321f1ffbc 12 const float TaskMoveToWaypoint::DEFAULT_VELOCITY = 0.2f; // default velocity value, given in [m/s]
Brignall 0:1a0321f1ffbc 13 const float TaskMoveToWaypoint::PI = 3.14159265f; // the constant PI
Brignall 0:1a0321f1ffbc 14 const float TaskMoveToWaypoint::K = 2.0f; // controller gain parameter
Brignall 0:1a0321f1ffbc 15
Brignall 0:1a0321f1ffbc 16 /**
Brignall 0:1a0321f1ffbc 17 * Creates a task object that moves the robot to a given waypoint.
Brignall 0:1a0321f1ffbc 18 * @param conroller a reference to the controller object of the robot.
Brignall 0:1a0321f1ffbc 19 * @param x the x coordinate of the waypoint position, given in [m].
Brignall 0:1a0321f1ffbc 20 * @param y the y coordinate of the waypoint position, given in [m].
Brignall 0:1a0321f1ffbc 21 */
Brignall 0:1a0321f1ffbc 22 TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y) : controller(controller) {
Brignall 0:1a0321f1ffbc 23
Brignall 0:1a0321f1ffbc 24 this->x = x;
Brignall 0:1a0321f1ffbc 25 this->y = y;
Brignall 0:1a0321f1ffbc 26 this->velocity = DEFAULT_VELOCITY;
Brignall 0:1a0321f1ffbc 27 this->initialDistance = 0.0f;
Brignall 0:1a0321f1ffbc 28 }
Brignall 0:1a0321f1ffbc 29
Brignall 0:1a0321f1ffbc 30 /**
Brignall 0:1a0321f1ffbc 31 * Creates a task object that moves the robot to a given waypoint.
Brignall 0:1a0321f1ffbc 32 * @param conroller a reference to the controller object of the robot.
Brignall 0:1a0321f1ffbc 33 * @param x the x coordinate of the target position, given in [m].
Brignall 0:1a0321f1ffbc 34 * @param y the y coordinate of the target position, given in [m].
Brignall 0:1a0321f1ffbc 35 * @param velocity the maximum translational velocity, given in [m/s].
Brignall 0:1a0321f1ffbc 36 */
Brignall 0:1a0321f1ffbc 37 TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y, float velocity) : controller(controller) {
Brignall 0:1a0321f1ffbc 38
Brignall 0:1a0321f1ffbc 39 this->x = x;
Brignall 0:1a0321f1ffbc 40 this->y = y;
Brignall 0:1a0321f1ffbc 41 this->velocity = velocity;
Brignall 0:1a0321f1ffbc 42 this->initialDistance = 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 TaskMoveToWaypoint::~TaskMoveToWaypoint() {}
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 TaskMoveToWaypoint::run(float period) {
Brignall 0:1a0321f1ffbc 56
Brignall 0:1a0321f1ffbc 57 float translationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 58 float rotationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 59
Brignall 0:1a0321f1ffbc 60 float x = controller.getX();
Brignall 0:1a0321f1ffbc 61 float y = controller.getY();
Brignall 0:1a0321f1ffbc 62 float alpha = controller.getAlpha();
Brignall 0:1a0321f1ffbc 63
Brignall 0:1a0321f1ffbc 64 float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y));
Brignall 0:1a0321f1ffbc 65 float gamma = atan2(this->y-y, this->x-x)-alpha;
Brignall 0:1a0321f1ffbc 66
Brignall 0:1a0321f1ffbc 67 while (gamma < -PI) gamma += 2.0f*PI;
Brignall 0:1a0321f1ffbc 68 while (gamma > PI) gamma -= 2.0f*PI;
Brignall 0:1a0321f1ffbc 69
Brignall 0:1a0321f1ffbc 70 if (initialDistance == 0.0f) initialDistance = rho;
Brignall 0:1a0321f1ffbc 71
Brignall 0:1a0321f1ffbc 72 translationalVelocity = velocity;
Brignall 0:1a0321f1ffbc 73 rotationalVelocity = (rho < initialDistance/2.0f) ? 0.0f : K*gamma;
Brignall 0:1a0321f1ffbc 74
Brignall 0:1a0321f1ffbc 75 controller.setTranslationalVelocity(translationalVelocity);
Brignall 0:1a0321f1ffbc 76 controller.setRotationalVelocity(rotationalVelocity);
Brignall 0:1a0321f1ffbc 77
Brignall 0:1a0321f1ffbc 78 return (rho < initialDistance/2.0f) && ((gamma < -PI/2.0f) || (gamma > PI/2.0f)) ? DONE : RUNNING;
Brignall 0:1a0321f1ffbc 79 }
Brignall 0:1a0321f1ffbc 80