ROME2 Lab3
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 16:04:31 by
1.7.2