Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

TaskMoveTo.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

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