Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

TaskMoveTo.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

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