ROME2 Lab3

Committer:
oehlemar
Date:
Wed Mar 25 14:15:52 2020 +0000
Revision:
2:fc9e2aebf9d5
Parent:
1:ff05970bb9b0
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:6a4d3264c067 1 /*
oehlemar 0:6a4d3264c067 2 * TaskMoveTo.cpp
oehlemar 0:6a4d3264c067 3 * Copyright (c) 2020, ZHAW
oehlemar 0:6a4d3264c067 4 * All rights reserved.
oehlemar 0:6a4d3264c067 5 */
oehlemar 0:6a4d3264c067 6
oehlemar 0:6a4d3264c067 7 #include <cmath>
oehlemar 0:6a4d3264c067 8 #include "TaskMoveTo.h"
oehlemar 0:6a4d3264c067 9
oehlemar 0:6a4d3264c067 10
oehlemar 0:6a4d3264c067 11 using namespace std;
oehlemar 0:6a4d3264c067 12
oehlemar 0:6a4d3264c067 13 const float TaskMoveTo::DEFAULT_VELOCITY = 0.5f; // default velocity value, given in [m/s]
oehlemar 0:6a4d3264c067 14 const float TaskMoveTo::DEFAULT_ZONE = 0.01f; // default zone value, given in [m]
oehlemar 0:6a4d3264c067 15 const float TaskMoveTo::PI = 3.14159265f; // the constant PI
oehlemar 0:6a4d3264c067 16 const float TaskMoveTo::K1 = 2.0f; // position controller gain parameter
oehlemar 0:6a4d3264c067 17 const float TaskMoveTo::K2 = 2.0f; // position controller gain parameter
oehlemar 0:6a4d3264c067 18 const float TaskMoveTo::K3 = 1.0f; // position controller gain parameter
oehlemar 0:6a4d3264c067 19
oehlemar 0:6a4d3264c067 20 /**
oehlemar 0:6a4d3264c067 21 * Creates a task object that moves the robot to a given pose.
oehlemar 0:6a4d3264c067 22 * @param conroller a reference to the controller object of the robot.
oehlemar 0:6a4d3264c067 23 * @param x the x coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 24 * @param y the y coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 25 * @param alpha the target orientation, given in [rad].
oehlemar 0:6a4d3264c067 26 */
oehlemar 0:6a4d3264c067 27 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
oehlemar 0:6a4d3264c067 28
oehlemar 0:6a4d3264c067 29 this->x = x;
oehlemar 0:6a4d3264c067 30 this->y = y;
oehlemar 0:6a4d3264c067 31 this->alpha = alpha;
oehlemar 0:6a4d3264c067 32 this->velocity = DEFAULT_VELOCITY;
oehlemar 0:6a4d3264c067 33 this->zone = DEFAULT_ZONE;
oehlemar 0:6a4d3264c067 34 }
oehlemar 0:6a4d3264c067 35
oehlemar 0:6a4d3264c067 36 /**
oehlemar 0:6a4d3264c067 37 * Creates a task object that moves the robot to a given pose.
oehlemar 0:6a4d3264c067 38 * @param conroller a reference to the controller object of the robot.
oehlemar 0:6a4d3264c067 39 * @param x the x coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 40 * @param y the y coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 41 * @param alpha the target orientation, given in [rad].
oehlemar 0:6a4d3264c067 42 * @param velocity the maximum translational velocity, given in [m/s].
oehlemar 0:6a4d3264c067 43 */
oehlemar 0:6a4d3264c067 44 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
oehlemar 0:6a4d3264c067 45
oehlemar 0:6a4d3264c067 46 this->x = x;
oehlemar 0:6a4d3264c067 47 this->y = y;
oehlemar 0:6a4d3264c067 48 this->alpha = alpha;
oehlemar 0:6a4d3264c067 49 this->velocity = velocity;
oehlemar 0:6a4d3264c067 50 this->zone = DEFAULT_ZONE;
oehlemar 0:6a4d3264c067 51 }
oehlemar 0:6a4d3264c067 52
oehlemar 0:6a4d3264c067 53 /**
oehlemar 0:6a4d3264c067 54 * Creates a task object that moves the robot to a given pose.
oehlemar 0:6a4d3264c067 55 * @param conroller a reference to the controller object of the robot.
oehlemar 0:6a4d3264c067 56 * @param x the x coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 57 * @param y the y coordinate of the target position, given in [m].
oehlemar 0:6a4d3264c067 58 * @param alpha the target orientation, given in [rad].
oehlemar 0:6a4d3264c067 59 * @param velocity the maximum translational velocity, given in [m/s].
oehlemar 0:6a4d3264c067 60 * @param zone the zone threshold around the target position, given in [m].
oehlemar 0:6a4d3264c067 61 */
oehlemar 0:6a4d3264c067 62 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
oehlemar 0:6a4d3264c067 63
oehlemar 0:6a4d3264c067 64 this->x = x;
oehlemar 0:6a4d3264c067 65 this->y = y;
oehlemar 0:6a4d3264c067 66 this->alpha = alpha;
oehlemar 0:6a4d3264c067 67 this->velocity = velocity;
oehlemar 0:6a4d3264c067 68 this->zone = zone;
oehlemar 0:6a4d3264c067 69 }
oehlemar 0:6a4d3264c067 70
oehlemar 0:6a4d3264c067 71 /**
oehlemar 0:6a4d3264c067 72 * Deletes the task object.
oehlemar 0:6a4d3264c067 73 */
oehlemar 0:6a4d3264c067 74 TaskMoveTo::~TaskMoveTo() {}
oehlemar 0:6a4d3264c067 75
oehlemar 0:6a4d3264c067 76 /**
oehlemar 0:6a4d3264c067 77 * This method is called periodically by a task sequencer.
oehlemar 0:6a4d3264c067 78 * @param period the period of the task sequencer, given in [s].
oehlemar 0:6a4d3264c067 79 * @return the status of this task, i.e. RUNNING or DONE.
oehlemar 0:6a4d3264c067 80 */
oehlemar 0:6a4d3264c067 81 int TaskMoveTo::run(float period) {
oehlemar 0:6a4d3264c067 82
oehlemar 2:fc9e2aebf9d5 83
oehlemar 0:6a4d3264c067 84
oehlemar 0:6a4d3264c067 85 // bitte implementieren!
oehlemar 0:6a4d3264c067 86
oehlemar 2:fc9e2aebf9d5 87 double rho = sqrt(pow(x-controller.getX(),2)+pow(y-controller.getY(),2)); // if rho < 0.001 return done else return running
oehlemar 2:fc9e2aebf9d5 88
oehlemar 1:ff05970bb9b0 89
oehlemar 2:fc9e2aebf9d5 90 if (rho < 0.00001) { //double nicht mit 0 verlgeichen, if statement für ob man schon an gewünschter koordinate ist oder nicht
oehlemar 2:fc9e2aebf9d5 91 return DONE;
oehlemar 2:fc9e2aebf9d5 92 } else {
oehlemar 2:fc9e2aebf9d5 93 return RUNNING;
oehlemar 2:fc9e2aebf9d5 94 }
oehlemar 1:ff05970bb9b0 95
oehlemar 0:6a4d3264c067 96 double gamma = atan2(y-controller.getY(),x-controller.getX())- controller.getAlpha();
oehlemar 0:6a4d3264c067 97 double delta = gamma + controller.getAlpha()-alpha;
oehlemar 1:ff05970bb9b0 98 double beta;
oehlemar 0:6a4d3264c067 99
oehlemar 1:ff05970bb9b0 100 if (gamma < -PI ){
oehlemar 1:ff05970bb9b0 101 gamma += 2*PI;}
oehlemar 1:ff05970bb9b0 102 else if(gamma > PI){
oehlemar 1:ff05970bb9b0 103 gamma -= 2*PI;}
oehlemar 1:ff05970bb9b0 104 else if(delta < -PI ){
oehlemar 1:ff05970bb9b0 105 delta += 2*PI;}
oehlemar 1:ff05970bb9b0 106 else if(delta > PI){
oehlemar 1:ff05970bb9b0 107 delta -= 2*PI;}
oehlemar 2:fc9e2aebf9d5 108 else if (gamma <= 0.000001){
oehlemar 1:ff05970bb9b0 109 beta = 0;}
oehlemar 1:ff05970bb9b0 110 else {}
oehlemar 1:ff05970bb9b0 111
oehlemar 2:fc9e2aebf9d5 112 beta = K2*gamma+K1*sin(gamma)*cos(gamma)*((gamma+K3*delta)/gamma); //falsche formel
oehlemar 2:fc9e2aebf9d5 113 double x_trans = K1*rho*cos(gamma);
oehlemar 1:ff05970bb9b0 114
oehlemar 2:fc9e2aebf9d5 115 controller.setTranslationalVelocity(x_trans); //GESCHIWNDIGKEITEN NICHT POSITION
oehlemar 2:fc9e2aebf9d5 116 controller.setRotationalVelocity(beta);
oehlemar 0:6a4d3264c067 117
oehlemar 0:6a4d3264c067 118 return DONE;
oehlemar 0:6a4d3264c067 119 }
oehlemar 0:6a4d3264c067 120