Marco Oehler / Mbed OS Lab3

ROME2 Lab3

TaskMoveTo.cpp@0:6a4d3264c067, 2020-03-24 (annotated)

Committer:
oehlemar
Date:
Tue Mar 24 08:39:54 2020 +0000
Revision:
0:6a4d3264c067
Child:
1:ff05970bb9b0
Lab3

Who changed what in which revision?

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