ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

TaskMoveTo.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

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