TaskMoveToWaypoint.cpp

00001 /*
00002  * TaskMoveToWaypoint.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "TaskMoveToWaypoint.h"
00009 
00010 using namespace std;
00011 
00012 const float TaskMoveToWaypoint::DEFAULT_VELOCITY = 0.2f;    // default velocity value, given in [m/s]
00013 const float TaskMoveToWaypoint::PI = 3.14159265f;           // the constant PI
00014 const float TaskMoveToWaypoint::K = 2.0f;                   // controller gain parameter
00015 
00016 /**
00017  * Creates a task object that moves the robot to a given waypoint.
00018  * @param conroller a reference to the controller object of the robot.
00019  * @param x the x coordinate of the waypoint position, given in [m].
00020  * @param y the y coordinate of the waypoint position, given in [m].
00021  */
00022 TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y) : controller(controller) {
00023     
00024     this->x = x;
00025     this->y = y;
00026     this->velocity = DEFAULT_VELOCITY;
00027     this->initialDistance = 0.0f;
00028 }
00029 
00030 /**
00031  * Creates a task object that moves the robot to a given waypoint.
00032  * @param conroller a reference to the controller object of the robot.
00033  * @param x the x coordinate of the target position, given in [m].
00034  * @param y the y coordinate of the target position, given in [m].
00035  * @param velocity the maximum translational velocity, given in [m/s].
00036  */
00037 TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y, float velocity) : controller(controller) {
00038     
00039     this->x = x;
00040     this->y = y;
00041     this->velocity = velocity;
00042     this->initialDistance = 0.0f;
00043 }
00044 
00045 /**
00046  * Deletes the task object.
00047  */
00048 TaskMoveToWaypoint::~TaskMoveToWaypoint() {}
00049 
00050 /**
00051  * This method is called periodically by a task sequencer.
00052  * @param period the period of the task sequencer, given in [s].
00053  * @return the status of this task, i.e. RUNNING or DONE.
00054  */
00055 int TaskMoveToWaypoint::run(float period) {
00056     
00057     float translationalVelocity = 0.0f;
00058     float rotationalVelocity = 0.0f;
00059     
00060     float x = controller.getX();
00061     float y = controller.getY();
00062     float alpha = controller.getAlpha();
00063     
00064     float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y));
00065     float gamma = atan2(this->y-y, this->x-x)-alpha;
00066     
00067     while (gamma < -PI) gamma += 2.0f*PI;
00068     while (gamma > PI) gamma -= 2.0f*PI;
00069     
00070     if (initialDistance == 0.0f) initialDistance = rho;
00071     
00072     translationalVelocity = velocity;
00073     rotationalVelocity = (rho < initialDistance/2.0f) ? 0.0f : K*gamma;
00074     
00075     controller.setTranslationalVelocity(translationalVelocity);
00076     controller.setRotationalVelocity(rotationalVelocity);
00077     
00078     return (rho < initialDistance/2.0f) && ((gamma < -PI/2.0f) || (gamma > PI/2.0f)) ? DONE : RUNNING;
00079 }
00080