TeamSurface / 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

Library/TaskMoveTo.cpp@1:7bf9b6c007a1, 2018-03-23 (annotated)

Committer:
kueenste
Date:
Fri Mar 23 15:40:09 2018 +0000
Revision:
1:7bf9b6c007a1
Parent:
0:7cf5bf7e9486 
schissdreck funktioniert so halbe

Who changed what in which revision?

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