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 rho = 0.0f;
00083     float gamma = 0.0f;
00084     float delta = 0.0f;
00085     
00086     rho = sqrt((x-controller.getX())*(x-controller.getX())+(y-controller.getY())*(y-controller.getY()));
00087     
00088     if(rho >= 0.0f){
00089         gamma = atan2(y-controller.getY(),x-controller.getX())-controller.getAlpha();
00090         delta = gamma + controller.getAlpha() - alpha;
00091         }
00092     if(gamma<-PI){
00093         gamma = gamma+2.0f*PI;
00094         }
00095     if(gamma>PI){
00096         gamma = gamma-2.0f*PI;
00097         }
00098     if(delta<-PI){
00099         delta = delta+2.0f*PI;
00100         }
00101     if(delta>PI){
00102         delta = delta-2.0f*PI;
00103         }
00104     if(gamma == 0.0f){
00105         controller.setRotationalVelocity(0.0f);
00106         }
00107     else{
00108         controller.setRotationalVelocity(K2*gamma+K1*sin(gamma)*(gamma+K3*delta)/gamma);
00109         }
00110     controller.setTranslationalVelocity(K1*rho*cos(gamma));
00111     
00112     if(rho <= DEFAULT_ZONE){
00113         return DONE;
00114         }
00115         
00116         
00117     return RUNNING;
00118     
00119     
00120     
00121     // bitte implementieren!
00122 }
00123