Diff: TaskMoveTo.cpp

Revision:

0:20ec9d702676

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveTo.cpp	Tue Mar 31 11:58:30 2020 +0000
@@ -0,0 +1,130 @@
+/*
+ * TaskMoveTo.cpp
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMoveTo.h"
+
+using namespace std;
+
+const float TaskMoveTo::DEFAULT_VELOCITY = 0.5f;    // default velocity value, given in [m/s]
+const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
+const float TaskMoveTo::PI = 3.14159265f;           // the constant PI
+const float TaskMoveTo::K1 = 2.0f;                  // position controller gain parameter
+const float TaskMoveTo::K2 = 2.0f;                  // position controller gain parameter
+const float TaskMoveTo::K3 = 1.0f;                  // position controller gain parameter
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @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 alpha the target orientation, given in [rad].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = DEFAULT_VELOCITY;
+    this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @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 alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = velocity;
+    this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @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 alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ * @param zone the zone threshold around the target position, given in [m].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = velocity;
+    this->zone = zone;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMoveTo::~TaskMoveTo() {}
+
+/**
+ * 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 TaskMoveTo::run(float period) {
+     
+     float roh = 0;
+     float gam = 0;
+     float delat = 0;
+    
+    roh = sqrt((x - controller.getX())*(x - controller.getX())+(y - controller.getY())*(y - controller.getY()));
+    
+    if (roh >= 0.01f) {
+        gam = atan2(y - controller.getY(),x - controller.getX())-controller.getAlpha();
+        delta = gam + controller.getAlpha - alpha;
+       
+        
+        while (gam < -PI) {
+            gam = gam + 2* PI;
+        {
+            
+        while (gam > PI) {
+            gam = gam - 2 * PI;
+        }
+            
+        while  (delta < PI) {
+            delta = delta + 2*PI;
+        }
+        
+        while (delta > PI) {
+            delta = delta - 2* PI;
+        }
+        
+        if (fabs(gam) <= 0.0005f){
+        controller.setRotationVelocity(0);
+        }else {
+            controller.setRotationVelocity(K2*gam+K1*sin(gam)*cos(gam) * ((gam+K3*delta)/gam);
+        }
+        controller.setTranslationVelocity(K1*roh*cos(gam));
+        return RUNNING;
+        
+         
+        
+        }else {
+            controller.setRotationVelocity(0.0f);
+            controller.setRotationVelocity(0.0f);
+            return DONE;    
+        }
+        
+    
+    
+
+
+
+    
\ No newline at end of file