ROME2 Lab3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TaskMoveTo.cpp Source File

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 
00011 using namespace std;
00012 
00013 const float TaskMoveTo::DEFAULT_VELOCITY = 0.5f;    // default velocity value, given in [m/s]
00014 const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
00015 const float TaskMoveTo::PI = 3.14159265f;           // the constant PI
00016 const float TaskMoveTo::K1 = 2.0f;                  // position controller gain parameter
00017 const float TaskMoveTo::K2 = 2.0f;                  // position controller gain parameter
00018 const float TaskMoveTo::K3 = 1.0f;                  // position controller gain parameter
00019 
00020 /**
00021  * Creates a task object that moves the robot to a given pose.
00022  * @param conroller a reference to the controller object of the robot.
00023  * @param x the x coordinate of the target position, given in [m].
00024  * @param y the y coordinate of the target position, given in [m].
00025  * @param alpha the target orientation, given in [rad].
00026  */
00027 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
00028     
00029     this->x = x;
00030     this->y = y;
00031     this->alpha = alpha;
00032     this->velocity = DEFAULT_VELOCITY;
00033     this->zone = DEFAULT_ZONE;
00034 }
00035 
00036 /**
00037  * Creates a task object that moves the robot to a given pose.
00038  * @param conroller a reference to the controller object of the robot.
00039  * @param x the x coordinate of the target position, given in [m].
00040  * @param y the y coordinate of the target position, given in [m].
00041  * @param alpha the target orientation, given in [rad].
00042  * @param velocity the maximum translational velocity, given in [m/s].
00043  */
00044 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
00045     
00046     this->x = x;
00047     this->y = y;
00048     this->alpha = alpha;
00049     this->velocity = velocity;
00050     this->zone = DEFAULT_ZONE;
00051 }
00052 
00053 /**
00054  * Creates a task object that moves the robot to a given pose.
00055  * @param conroller a reference to the controller object of the robot.
00056  * @param x the x coordinate of the target position, given in [m].
00057  * @param y the y coordinate of the target position, given in [m].
00058  * @param alpha the target orientation, given in [rad].
00059  * @param velocity the maximum translational velocity, given in [m/s].
00060  * @param zone the zone threshold around the target position, given in [m].
00061  */
00062 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
00063     
00064     this->x = x;
00065     this->y = y;
00066     this->alpha = alpha;
00067     this->velocity = velocity;
00068     this->zone = zone;
00069 }
00070 
00071 /**
00072  * Deletes the task object.
00073  */
00074 TaskMoveTo::~TaskMoveTo() {}
00075 
00076 /**
00077  * This method is called periodically by a task sequencer.
00078  * @param period the period of the task sequencer, given in [s].
00079  * @return the status of this task, i.e. RUNNING or DONE.
00080  */
00081 int TaskMoveTo::run(float period) {
00082    
00083     
00084     
00085     // bitte implementieren!
00086     
00087     double rho = sqrt(pow(x-controller.getX(),2)+pow(y-controller.getY(),2)); // if rho < 0.001 return done else return running
00088     
00089     
00090     if (rho < 0.00001) {  //double nicht mit 0 verlgeichen, if statement für ob man schon an gewünschter koordinate ist oder nicht
00091         return DONE;
00092     } else { 
00093     return RUNNING;
00094     }
00095     
00096     double gamma = atan2(y-controller.getY(),x-controller.getX())- controller.getAlpha();
00097     double delta = gamma + controller.getAlpha()-alpha;
00098     double beta;
00099     
00100     if (gamma < -PI ){
00101         gamma += 2*PI;}
00102     else if(gamma > PI){
00103         gamma -= 2*PI;} 
00104     else if(delta < -PI ){
00105         delta += 2*PI;}
00106     else if(delta > PI){
00107         delta -= 2*PI;}  
00108     else if (gamma <= 0.000001){
00109              beta = 0;}
00110     else {}
00111         
00112     beta = K2*gamma+K1*sin(gamma)*cos(gamma)*((gamma+K3*delta)/gamma); //falsche formel
00113     double x_trans = K1*rho*cos(gamma);
00114      
00115     controller.setTranslationalVelocity(x_trans); //GESCHIWNDIGKEITEN NICHT POSITION
00116     controller.setRotationalVelocity(beta);
00117     
00118     return DONE;
00119 }
00120