Diff: TaskMoveToWaypoint.cpp

Revision:

0:1a0321f1ffbc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveToWaypoint.cpp	Fri May 18 12:18:21 2018 +0000
@@ -0,0 +1,80 @@
+/*
+ * TaskMoveToWaypoint.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMoveToWaypoint.h"
+
+using namespace std;
+
+const float TaskMoveToWaypoint::DEFAULT_VELOCITY = 0.2f;    // default velocity value, given in [m/s]
+const float TaskMoveToWaypoint::PI = 3.14159265f;           // the constant PI
+const float TaskMoveToWaypoint::K = 2.0f;                   // controller gain parameter
+
+/**
+ * Creates a task object that moves the robot to a given waypoint.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the waypoint position, given in [m].
+ * @param y the y coordinate of the waypoint position, given in [m].
+ */
+TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->velocity = DEFAULT_VELOCITY;
+    this->initialDistance = 0.0f;
+}
+
+/**
+ * Creates a task object that moves the robot to a given waypoint.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ */
+TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y, float velocity) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->velocity = velocity;
+    this->initialDistance = 0.0f;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMoveToWaypoint::~TaskMoveToWaypoint() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskMoveToWaypoint::run(float period) {
+    
+    float translationalVelocity = 0.0f;
+    float rotationalVelocity = 0.0f;
+    
+    float x = controller.getX();
+    float y = controller.getY();
+    float alpha = controller.getAlpha();
+    
+    float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y));
+    float gamma = atan2(this->y-y, this->x-x)-alpha;
+    
+    while (gamma < -PI) gamma += 2.0f*PI;
+    while (gamma > PI) gamma -= 2.0f*PI;
+    
+    if (initialDistance == 0.0f) initialDistance = rho;
+    
+    translationalVelocity = velocity;
+    rotationalVelocity = (rho < initialDistance/2.0f) ? 0.0f : K*gamma;
+    
+    controller.setTranslationalVelocity(translationalVelocity);
+    controller.setRotationalVelocity(rotationalVelocity);
+    
+    return (rho < initialDistance/2.0f) && ((gamma < -PI/2.0f) || (gamma > PI/2.0f)) ? DONE : RUNNING;
+}
+