TaskMoveTo.cpp

00001 /*
00002  * TaskMoveTo.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "TaskMoveTo.h"
00009 
00010 using namespace std;
00011 
00012 const float TaskMoveTo::DEFAULT_VELOCITY = 0.5f;    // default velocity value, given in [m/s]
00013 const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
00014 const float TaskMoveTo::PI = 3.14159265f;           // the constant PI
00015 const float TaskMoveTo::K1 = 2.0f;                  // position controller gain parameter
00016 const float TaskMoveTo::K2 = 2.0f;                  // position controller gain parameter
00017 const float TaskMoveTo::K3 = 1.0f;                  // position controller gain parameter
00018 
00019 /**
00020  * Creates a task object that moves the robot to a given pose.
00021  * @param conroller a reference to the controller object of the robot.
00022  * @param x the x coordinate of the target position, given in [m].
00023  * @param y the y coordinate of the target position, given in [m].
00024  * @param alpha the target orientation, given in [rad].
00025  */
00026 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
00027     
00028     this->x = x;
00029     this->y = y;
00030     this->alpha = alpha;
00031     this->velocity = DEFAULT_VELOCITY;
00032     this->zone = DEFAULT_ZONE;
00033 }
00034 
00035 /**
00036  * Creates a task object that moves the robot to a given pose.
00037  * @param conroller a reference to the controller object of the robot.
00038  * @param x the x coordinate of the target position, given in [m].
00039  * @param y the y coordinate of the target position, given in [m].
00040  * @param alpha the target orientation, given in [rad].
00041  * @param velocity the maximum translational velocity, given in [m/s].
00042  */
00043 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
00044     
00045     this->x = x;
00046     this->y = y;
00047     this->alpha = alpha;
00048     this->velocity = velocity;
00049     this->zone = DEFAULT_ZONE;
00050 }
00051 
00052 /**
00053  * Creates a task object that moves the robot to a given pose.
00054  * @param conroller a reference to the controller object of the robot.
00055  * @param x the x coordinate of the target position, given in [m].
00056  * @param y the y coordinate of the target position, given in [m].
00057  * @param alpha the target orientation, given in [rad].
00058  * @param velocity the maximum translational velocity, given in [m/s].
00059  * @param zone the zone threshold around the target position, given in [m].
00060  */
00061 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
00062     
00063     this->x = x;
00064     this->y = y;
00065     this->alpha = alpha;
00066     this->velocity = velocity;
00067     this->zone = zone;
00068 }
00069 
00070 /**
00071  * Deletes the task object.
00072  */
00073 TaskMoveTo::~TaskMoveTo() {}
00074 
00075 /**
00076  * This method is called periodically by a task sequencer.
00077  * @param period the period of the task sequencer, given in [s].
00078  * @return the status of this task, i.e. RUNNING or DONE.
00079  */
00080 int TaskMoveTo::run(float period) {
00081      
00082      float roh = 0;
00083      float gam = 0;
00084      float delat = 0;
00085     
00086     roh = sqrt((x - controller.getX())*(x - controller.getX())+(y - controller.getY())*(y - controller.getY()));
00087     
00088     if (roh >= 0.01f) {
00089         gam = atan2(y - controller.getY(),x - controller.getX())-controller.getAlpha();
00090         delta = gam + controller.getAlpha - alpha;
00091        
00092         
00093         while (gam < -PI) {
00094             gam = gam + 2* PI;
00095         {
00096             
00097         while (gam > PI) {
00098             gam = gam - 2 * PI;
00099         }
00100             
00101         while  (delta < PI) {
00102             delta = delta + 2*PI;
00103         }
00104         
00105         while (delta > PI) {
00106             delta = delta - 2* PI;
00107         }
00108         
00109         if (fabs(gam) <= 0.0005f){
00110         controller.setRotationVelocity(0);
00111         }else {
00112             controller.setRotationVelocity(K2*gam+K1*sin(gam)*cos(gam) * ((gam+K3*delta)/gam);
00113         }
00114         controller.setTranslationVelocity(K1*roh*cos(gam));
00115         return RUNNING;
00116         
00117          
00118         
00119         }else {
00120             controller.setRotationVelocity(0.0f);
00121             controller.setRotationVelocity(0.0f);
00122             return DONE;    
00123         }
00124         
00125     
00126     
00127 
00128 
00129 
00130