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@4:4428ede9beee, 2018-04-12 (annotated)

Committer:
matajarb
Date:
Thu Apr 12 12:19:19 2018 +0000
Revision:
4:4428ede9beee
Child:
5:59079b76ac7f
projektvorlag p3 wil de s?mme en schluch isch

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 4:4428ede9beee 82 // bitte implementieren!
matajarb 4:4428ede9beee 83
matajarb 4:4428ede9beee 84 return RUNNING;
matajarb 4:4428ede9beee 85 }
matajarb 4:4428ede9beee 86