TeamSurface / Libs

Nim leo niiiim

TaskMoveTo.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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