AlbaniGang / Mbed 2 deprecated Rome_P3

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:bba0ec7e075a, 2020-04-08 (annotated)

Committer:
stollpa1
Date:
Wed Apr 08 09:13:33 2020 +0000
Revision:
1:bba0ec7e075a
Parent:
0:0a667cdbf4c1 
P4 init;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wengefa1 0:0a667cdbf4c1 1 /*
wengefa1 0:0a667cdbf4c1 2 * TaskMoveTo.cpp
wengefa1 0:0a667cdbf4c1 3 * Copyright (c) 2020, ZHAW
wengefa1 0:0a667cdbf4c1 4 * All rights reserved.
wengefa1 0:0a667cdbf4c1 5 */
wengefa1 0:0a667cdbf4c1 6
wengefa1 0:0a667cdbf4c1 7 #include <cmath>
wengefa1 0:0a667cdbf4c1 8 #include "TaskMoveTo.h"
wengefa1 0:0a667cdbf4c1 9
wengefa1 0:0a667cdbf4c1 10 using namespace std;
wengefa1 0:0a667cdbf4c1 11
wengefa1 0:0a667cdbf4c1 12 const float TaskMoveTo::DEFAULT_VELOCITY = 0.5f; // default velocity value, given in [m/s]
wengefa1 0:0a667cdbf4c1 13 const float TaskMoveTo::DEFAULT_ZONE = 0.01f; // default zone value, given in [m]
wengefa1 0:0a667cdbf4c1 14 const float TaskMoveTo::PI = 3.14159265f; // the constant PI
wengefa1 0:0a667cdbf4c1 15 const float TaskMoveTo::K1 = 2.0f; // position controller gain parameter
wengefa1 0:0a667cdbf4c1 16 const float TaskMoveTo::K2 = 2.0f; // position controller gain parameter
wengefa1 0:0a667cdbf4c1 17 const float TaskMoveTo::K3 = 1.0f; // position controller gain parameter
wengefa1 0:0a667cdbf4c1 18
wengefa1 0:0a667cdbf4c1 19 /**
wengefa1 0:0a667cdbf4c1 20 * Creates a task object that moves the robot to a given pose.
wengefa1 0:0a667cdbf4c1 21 * @param conroller a reference to the controller object of the robot.
wengefa1 0:0a667cdbf4c1 22 * @param x the x coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 23 * @param y the y coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 24 * @param alpha the target orientation, given in [rad].
wengefa1 0:0a667cdbf4c1 25 */
wengefa1 0:0a667cdbf4c1 26 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
wengefa1 0:0a667cdbf4c1 27
wengefa1 0:0a667cdbf4c1 28 this->x = x;
wengefa1 0:0a667cdbf4c1 29 this->y = y;
wengefa1 0:0a667cdbf4c1 30 this->alpha = alpha;
wengefa1 0:0a667cdbf4c1 31 this->velocity = DEFAULT_VELOCITY;
wengefa1 0:0a667cdbf4c1 32 this->zone = DEFAULT_ZONE;
wengefa1 0:0a667cdbf4c1 33 }
wengefa1 0:0a667cdbf4c1 34
wengefa1 0:0a667cdbf4c1 35 /**
wengefa1 0:0a667cdbf4c1 36 * Creates a task object that moves the robot to a given pose.
wengefa1 0:0a667cdbf4c1 37 * @param conroller a reference to the controller object of the robot.
wengefa1 0:0a667cdbf4c1 38 * @param x the x coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 39 * @param y the y coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 40 * @param alpha the target orientation, given in [rad].
wengefa1 0:0a667cdbf4c1 41 * @param velocity the maximum translational velocity, given in [m/s].
wengefa1 0:0a667cdbf4c1 42 */
wengefa1 0:0a667cdbf4c1 43 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
wengefa1 0:0a667cdbf4c1 44
wengefa1 0:0a667cdbf4c1 45 this->x = x;
wengefa1 0:0a667cdbf4c1 46 this->y = y;
wengefa1 0:0a667cdbf4c1 47 this->alpha = alpha;
wengefa1 0:0a667cdbf4c1 48 this->velocity = velocity;
wengefa1 0:0a667cdbf4c1 49 this->zone = DEFAULT_ZONE;
wengefa1 0:0a667cdbf4c1 50 }
wengefa1 0:0a667cdbf4c1 51
wengefa1 0:0a667cdbf4c1 52 /**
wengefa1 0:0a667cdbf4c1 53 * Creates a task object that moves the robot to a given pose.
wengefa1 0:0a667cdbf4c1 54 * @param conroller a reference to the controller object of the robot.
wengefa1 0:0a667cdbf4c1 55 * @param x the x coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 56 * @param y the y coordinate of the target position, given in [m].
wengefa1 0:0a667cdbf4c1 57 * @param alpha the target orientation, given in [rad].
wengefa1 0:0a667cdbf4c1 58 * @param velocity the maximum translational velocity, given in [m/s].
wengefa1 0:0a667cdbf4c1 59 * @param zone the zone threshold around the target position, given in [m].
wengefa1 0:0a667cdbf4c1 60 */
wengefa1 0:0a667cdbf4c1 61 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
wengefa1 0:0a667cdbf4c1 62
wengefa1 0:0a667cdbf4c1 63 this->x = x;
wengefa1 0:0a667cdbf4c1 64 this->y = y;
wengefa1 0:0a667cdbf4c1 65 this->alpha = alpha;
wengefa1 0:0a667cdbf4c1 66 this->velocity = velocity;
wengefa1 0:0a667cdbf4c1 67 this->zone = zone;
wengefa1 0:0a667cdbf4c1 68 }
wengefa1 0:0a667cdbf4c1 69
wengefa1 0:0a667cdbf4c1 70 /**
wengefa1 0:0a667cdbf4c1 71 * Deletes the task object.
wengefa1 0:0a667cdbf4c1 72 */
wengefa1 0:0a667cdbf4c1 73 TaskMoveTo::~TaskMoveTo() {}
wengefa1 0:0a667cdbf4c1 74
wengefa1 0:0a667cdbf4c1 75 /**
wengefa1 0:0a667cdbf4c1 76 * This method is called periodically by a task sequencer.
wengefa1 0:0a667cdbf4c1 77 * @param period the period of the task sequencer, given in [s].
wengefa1 0:0a667cdbf4c1 78 * @return the status of this task, i.e. RUNNING or DONE.
wengefa1 0:0a667cdbf4c1 79 */
wengefa1 0:0a667cdbf4c1 80 int TaskMoveTo::run(float period) {
wengefa1 0:0a667cdbf4c1 81
wengefa1 0:0a667cdbf4c1 82 return RUNNING;
wengefa1 0:0a667cdbf4c1 83
wengefa1 0:0a667cdbf4c1 84 // bitte implementieren!
wengefa1 0:0a667cdbf4c1 85 }
wengefa1 0:0a667cdbf4c1 86