Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

TaskMoveTo.cpp@5:59079b76ac7f, 2018-04-12 (annotated)

Committer:
matajarb
Date:
Thu Apr 12 14:56:27 2018 +0000
Revision:
5:59079b76ac7f
Parent:
4:4428ede9beee
Child:
6:67263dc2c2cf
lutsched eieer

Who changed what in which revision?

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