ROME_Praktikum / Mbed 2 deprecated Rome_P_3

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@0:20ec9d702676, 2020-03-31 (annotated)

Committer:
Jacqueline
Date:
Tue Mar 31 11:58:30 2020 +0000
Revision:
0:20ec9d702676
Praktikum_3

Who changed what in which revision?

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