ROME2 - TI / Mbed 2 deprecated ROME2 - Praktikum

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:   mbed

TaskMoveTo.cpp@1:08ca9b208045, 2017-03-31 (annotated)

Committer:
solcager
Date:
Fri Mar 31 11:00:19 2017 +0000
Revision:
1:08ca9b208045
P3

Who changed what in which revision?

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