TaskMoveTo.cpp

00001 /*
00002  * TaskMoveTo.cpp
00003  * Copyright (c) 2017, 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.2f;    // default velocity value, given in [m/s]
00013 const float TaskMoveTo::DEFAULT_ZONE = 0.1f;       // 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 = 0.5f;                  // position controller gain parameter
00017 const float TaskMoveTo::K3 = 0.5f;                  // 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 
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 
00047     this->x = x;
00048     this->y = y;
00049     this->alpha = alpha;
00050     this->velocity = velocity;
00051     this->zone = DEFAULT_ZONE;
00052 }
00053 
00054 /**
00055  * Creates a task object that moves the robot to a given pose.
00056  * @param conroller a reference to the controller object of the robot.
00057  * @param x the x coordinate of the target position, given in [m].
00058  * @param y the y coordinate of the target position, given in [m].
00059  * @param alpha the target orientation, given in [rad].
00060  * @param velocity the maximum translational velocity, given in [m/s].
00061  * @param zone the zone threshold around the target position, given in [m].
00062  */
00063 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller)
00064 {
00065 
00066     this->x = x;
00067     this->y = y;
00068     this->alpha = alpha;
00069     this->velocity = velocity;
00070     this->zone = zone;
00071 }
00072 
00073 /**
00074  * Deletes the task object.
00075  */
00076 TaskMoveTo::~TaskMoveTo() {}
00077 
00078 /**
00079  * This method is called periodically by a task sequencer.
00080  * @param period the period of the task sequencer, given in [s].
00081  * @return the status of this task, i.e. RUNNING or DONE.
00082  */
00083 int TaskMoveTo::run(float period)
00084 {
00085 
00086     // ignore period (only because of Task.h vererbung)
00087 
00088     // bitte implementieren!
00089 
00090     float xtargminx = x-controller.getX();
00091     float ytargminy = y-controller.getY();
00092 
00093     float roh = sqrt(pow(xtargminx,2)+pow(ytargminy,2));
00094     float gamma = atan2(ytargminy,xtargminx) - controller.getAlpha();
00095     float delta = gamma+controller.getAlpha() - alpha ;
00096     
00097     if(gamma > PI) {
00098         gamma -= 2*PI;       
00099     } else if(gamma < -PI) {
00100         gamma += 2*PI;    
00101     }
00102     
00103     if(delta > PI) {
00104         delta -= 2*PI;       
00105     } else if(delta < -PI) {
00106         delta += 2*PI;    
00107     }
00108 
00109     controller.setTranslationalVelocity(K1*roh*cos(gamma));
00110     controller.setRotationalVelocity(K2*gamma+K1*sin(gamma)*cos(gamma)*(gamma+K3*delta)/gamma);
00111 
00112     if(abs(xtargminx)<=zone && abs(ytargminy)<=zone && abs(alpha-controller.getAlpha())<=zone) {
00113         return DONE;
00114     } else {
00115         return RUNNING;
00116     }
00117 
00118 }
00119