Diff: Library/TaskMoveTo.cpp

Revision:

1:7bf9b6c007a1

Parent:

0:7cf5bf7e9486

--- a/Library/TaskMoveTo.cpp	Fri Mar 23 13:07:58 2018 +0000
+++ b/Library/TaskMoveTo.cpp	Fri Mar 23 15:40:09 2018 +0000
@@ -10,7 +10,7 @@
 using namespace std;
 
 const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f;    // default velocity value, given in [m/s]
-const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
+const float TaskMoveTo::DEFAULT_ZONE = 0.1f;       // 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 = 0.5f;                  // position controller gain parameter
@@ -23,8 +23,9 @@
  * @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) {
-    
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller)
+{
+
     this->x = x;
     this->y = y;
     this->alpha = alpha;
@@ -40,8 +41,9 @@
  * @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) {
-    
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller)
+{
+
     this->x = x;
     this->y = y;
     this->alpha = alpha;
@@ -58,8 +60,9 @@
  * @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) {
-    
+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;
@@ -77,10 +80,40 @@
  * @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) {
+int TaskMoveTo::run(float period)
+{
+
+    // ignore period (only because of Task.h vererbung)
+
+    // bitte implementieren!
+
+    float xtargminx = x-controller.getX();
+    float ytargminy = y-controller.getY();
+
+    float roh = sqrt(pow(xtargminx,2)+pow(ytargminy,2));
+    float gamma = atan2(ytargminy,xtargminx) - controller.getAlpha();
+    float delta = gamma+controller.getAlpha() - alpha ;
     
-    // bitte implementieren!
-    
-    return RUNNING;
+    if(gamma > PI) {
+        gamma -= 2*PI;       
+    } else if(gamma < -PI) {
+        gamma += 2*PI;    
+    }
+    
+    if(delta > PI) {
+        delta -= 2*PI;       
+    } else if(delta < -PI) {
+        delta += 2*PI;    
+    }
+
+    controller.setTranslationalVelocity(K1*roh*cos(gamma));
+    controller.setRotationalVelocity(K2*gamma+K1*sin(gamma)*cos(gamma)*(gamma+K3*delta)/gamma);
+
+    if(abs(xtargminx)<=zone && abs(ytargminy)<=zone && abs(alpha-controller.getAlpha())<=zone) {
+        return DONE;
+    } else {
+        return RUNNING;
+    }
+
 }