TeamSurface / Praktikum_3

Mit TaskWait ;-)

TaskMoveTo.cpp@0:92d57d5d9305, 2018-03-23 (annotated)

Committer:
wannesim
Date:
Fri Mar 23 15:44:15 2018 +0000
Revision:
0:92d57d5d9305
Mit TaskWait     ;-)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wannesim 0:92d57d5d9305 1 /*
wannesim 0:92d57d5d9305 2 * TaskMoveTo.cpp
wannesim 0:92d57d5d9305 3 * Copyright (c) 2017, ZHAW
wannesim 0:92d57d5d9305 4 * All rights reserved.
wannesim 0:92d57d5d9305 5 */
wannesim 0:92d57d5d9305 6
wannesim 0:92d57d5d9305 7 #include <cmath>
wannesim 0:92d57d5d9305 8 #include "TaskMoveTo.h"
wannesim 0:92d57d5d9305 9
wannesim 0:92d57d5d9305 10 using namespace std;
wannesim 0:92d57d5d9305 11
wannesim 0:92d57d5d9305 12 const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f; // default velocity value, given in [m/s]
wannesim 0:92d57d5d9305 13 const float TaskMoveTo::DEFAULT_ZONE = 0.05f; // default zone value, given in [m]
wannesim 0:92d57d5d9305 14 const float TaskMoveTo::PI = 3.14159265f; // the constant PI
wannesim 0:92d57d5d9305 15 const float TaskMoveTo::K1 = 2.0f; // position controller gain parameter
wannesim 0:92d57d5d9305 16 const float TaskMoveTo::K2 = 0.5f; // position controller gain parameter
wannesim 0:92d57d5d9305 17 const float TaskMoveTo::K3 = 0.5f; // position controller gain parameter
wannesim 0:92d57d5d9305 18
wannesim 0:92d57d5d9305 19 /**
wannesim 0:92d57d5d9305 20 * Creates a task object that moves the robot to a given pose.
wannesim 0:92d57d5d9305 21 * @param conroller a reference to the controller object of the robot.
wannesim 0:92d57d5d9305 22 * @param x the x coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 23 * @param y the y coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 24 * @param alpha the target orientation, given in [rad].
wannesim 0:92d57d5d9305 25 */
wannesim 0:92d57d5d9305 26 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
wannesim 0:92d57d5d9305 27
wannesim 0:92d57d5d9305 28 this->x = x;
wannesim 0:92d57d5d9305 29 this->y = y;
wannesim 0:92d57d5d9305 30 this->alpha = alpha;
wannesim 0:92d57d5d9305 31 this->velocity = DEFAULT_VELOCITY;
wannesim 0:92d57d5d9305 32 this->zone = DEFAULT_ZONE;
wannesim 0:92d57d5d9305 33 }
wannesim 0:92d57d5d9305 34
wannesim 0:92d57d5d9305 35 /**
wannesim 0:92d57d5d9305 36 * Creates a task object that moves the robot to a given pose.
wannesim 0:92d57d5d9305 37 * @param conroller a reference to the controller object of the robot.
wannesim 0:92d57d5d9305 38 * @param x the x coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 39 * @param y the y coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 40 * @param alpha the target orientation, given in [rad].
wannesim 0:92d57d5d9305 41 * @param velocity the maximum translational velocity, given in [m/s].
wannesim 0:92d57d5d9305 42 */
wannesim 0:92d57d5d9305 43 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
wannesim 0:92d57d5d9305 44
wannesim 0:92d57d5d9305 45 this->x = x;
wannesim 0:92d57d5d9305 46 this->y = y;
wannesim 0:92d57d5d9305 47 this->alpha = alpha;
wannesim 0:92d57d5d9305 48 this->velocity = velocity;
wannesim 0:92d57d5d9305 49 this->zone = DEFAULT_ZONE;
wannesim 0:92d57d5d9305 50 }
wannesim 0:92d57d5d9305 51
wannesim 0:92d57d5d9305 52 /**
wannesim 0:92d57d5d9305 53 * Creates a task object that moves the robot to a given pose.
wannesim 0:92d57d5d9305 54 * @param conroller a reference to the controller object of the robot.
wannesim 0:92d57d5d9305 55 * @param x the x coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 56 * @param y the y coordinate of the target position, given in [m].
wannesim 0:92d57d5d9305 57 * @param alpha the target orientation, given in [rad].
wannesim 0:92d57d5d9305 58 * @param velocity the maximum translational velocity, given in [m/s].
wannesim 0:92d57d5d9305 59 * @param zone the zone threshold around the target position, given in [m].
wannesim 0:92d57d5d9305 60 */
wannesim 0:92d57d5d9305 61 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
wannesim 0:92d57d5d9305 62
wannesim 0:92d57d5d9305 63 this->x = x;
wannesim 0:92d57d5d9305 64 this->y = y;
wannesim 0:92d57d5d9305 65 this->alpha = alpha;
wannesim 0:92d57d5d9305 66 this->velocity = velocity;
wannesim 0:92d57d5d9305 67 this->zone = zone;
wannesim 0:92d57d5d9305 68 }
wannesim 0:92d57d5d9305 69
wannesim 0:92d57d5d9305 70 /**
wannesim 0:92d57d5d9305 71 * Deletes the task object.
wannesim 0:92d57d5d9305 72 */
wannesim 0:92d57d5d9305 73 TaskMoveTo::~TaskMoveTo() {}
wannesim 0:92d57d5d9305 74
wannesim 0:92d57d5d9305 75 /**
wannesim 0:92d57d5d9305 76 * This method is called periodically by a task sequencer.
wannesim 0:92d57d5d9305 77 * @param period the period of the task sequencer, given in [s].
wannesim 0:92d57d5d9305 78 * @return the status of this task, i.e. RUNNING or DONE.
wannesim 0:92d57d5d9305 79 */
wannesim 0:92d57d5d9305 80 int TaskMoveTo::run(float period) {
wannesim 0:92d57d5d9305 81
wannesim 0:92d57d5d9305 82 // Abstand und Winkel zum Ziel
wannesim 0:92d57d5d9305 83 float roh = sqrt(pow((x - controller.getX()),2) + pow((y - controller.getY()),2));
wannesim 0:92d57d5d9305 84 float gamma = atan2(y - controller.getY(), x - controller.getX()) - controller.getAlpha();
wannesim 0:92d57d5d9305 85 if (gamma > PI) {
wannesim 0:92d57d5d9305 86 gamma = gamma - 2.0f*PI;
wannesim 0:92d57d5d9305 87 } else if (gamma < -PI) {
wannesim 0:92d57d5d9305 88 gamma = gamma + 2.0f*PI;
wannesim 0:92d57d5d9305 89 }
wannesim 0:92d57d5d9305 90 float delta = gamma + controller.getAlpha() - alpha;
wannesim 0:92d57d5d9305 91 if (delta > PI) {
wannesim 0:92d57d5d9305 92 delta = delta - 2.0f*PI;
wannesim 0:92d57d5d9305 93 } else if (delta < -PI) {
wannesim 0:92d57d5d9305 94 delta = delta + 2.0f*PI;
wannesim 0:92d57d5d9305 95 }
wannesim 0:92d57d5d9305 96
wannesim 0:92d57d5d9305 97 // Positionsregelung
wannesim 0:92d57d5d9305 98 controller.setTranslationalVelocity(K1 * roh * cos(gamma));
wannesim 0:92d57d5d9305 99 controller.setRotationalVelocity(K2 * gamma + K1 * sin(gamma) * cos(gamma) * ((gamma + K3 * delta)/gamma));
wannesim 0:92d57d5d9305 100
wannesim 0:92d57d5d9305 101 if (abs(x - controller.getX()) <= zone && abs(y - controller.getY()) <= zone && abs(x - controller.getAlpha()) <= zone) {
wannesim 0:92d57d5d9305 102 return DONE;
wannesim 0:92d57d5d9305 103 }
wannesim 0:92d57d5d9305 104 else {
wannesim 0:92d57d5d9305 105 return RUNNING;
wannesim 0:92d57d5d9305 106 }
wannesim 0:92d57d5d9305 107 }