rome2_p6 imported
Dependencies: mbed
TaskMoveToWaypoint.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
Appalco | 0:351a2fb21235 | 1 | /* |
Appalco | 0:351a2fb21235 | 2 | * TaskMoveToWaypoint.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 "TaskMoveToWaypoint.h" |
Appalco | 0:351a2fb21235 | 9 | |
Appalco | 0:351a2fb21235 | 10 | using namespace std; |
Appalco | 0:351a2fb21235 | 11 | |
Appalco | 0:351a2fb21235 | 12 | const float TaskMoveToWaypoint::DEFAULT_VELOCITY = 0.2f; // default velocity value, given in [m/s] |
Appalco | 0:351a2fb21235 | 13 | const float TaskMoveToWaypoint::PI = 3.14159265f; // the constant PI |
Appalco | 0:351a2fb21235 | 14 | const float TaskMoveToWaypoint::K = 2.0f; // controller gain parameter |
Appalco | 0:351a2fb21235 | 15 | |
Appalco | 0:351a2fb21235 | 16 | /** |
Appalco | 0:351a2fb21235 | 17 | * Creates a task object that moves the robot to a given waypoint. |
Appalco | 0:351a2fb21235 | 18 | * @param conroller a reference to the controller object of the robot. |
Appalco | 0:351a2fb21235 | 19 | * @param x the x coordinate of the waypoint position, given in [m]. |
Appalco | 0:351a2fb21235 | 20 | * @param y the y coordinate of the waypoint position, given in [m]. |
Appalco | 0:351a2fb21235 | 21 | */ |
Appalco | 0:351a2fb21235 | 22 | TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y) : controller(controller) { |
Appalco | 0:351a2fb21235 | 23 | |
Appalco | 0:351a2fb21235 | 24 | this->x = x; |
Appalco | 0:351a2fb21235 | 25 | this->y = y; |
Appalco | 0:351a2fb21235 | 26 | this->velocity = DEFAULT_VELOCITY; |
Appalco | 0:351a2fb21235 | 27 | this->initialDistance = 0.0f; |
Appalco | 0:351a2fb21235 | 28 | } |
Appalco | 0:351a2fb21235 | 29 | |
Appalco | 0:351a2fb21235 | 30 | /** |
Appalco | 0:351a2fb21235 | 31 | * Creates a task object that moves the robot to a given waypoint. |
Appalco | 0:351a2fb21235 | 32 | * @param conroller a reference to the controller object of the robot. |
Appalco | 0:351a2fb21235 | 33 | * @param x the x coordinate of the target position, given in [m]. |
Appalco | 0:351a2fb21235 | 34 | * @param y the y coordinate of the target position, given in [m]. |
Appalco | 0:351a2fb21235 | 35 | * @param velocity the maximum translational velocity, given in [m/s]. |
Appalco | 0:351a2fb21235 | 36 | */ |
Appalco | 0:351a2fb21235 | 37 | TaskMoveToWaypoint::TaskMoveToWaypoint(Controller& controller, float x, float y, float velocity) : controller(controller) { |
Appalco | 0:351a2fb21235 | 38 | |
Appalco | 0:351a2fb21235 | 39 | this->x = x; |
Appalco | 0:351a2fb21235 | 40 | this->y = y; |
Appalco | 0:351a2fb21235 | 41 | this->velocity = velocity; |
Appalco | 0:351a2fb21235 | 42 | this->initialDistance = 0.0f; |
Appalco | 0:351a2fb21235 | 43 | } |
Appalco | 0:351a2fb21235 | 44 | |
Appalco | 0:351a2fb21235 | 45 | /** |
Appalco | 0:351a2fb21235 | 46 | * Deletes the task object. |
Appalco | 0:351a2fb21235 | 47 | */ |
Appalco | 0:351a2fb21235 | 48 | TaskMoveToWaypoint::~TaskMoveToWaypoint() {} |
Appalco | 0:351a2fb21235 | 49 | |
Appalco | 0:351a2fb21235 | 50 | /** |
Appalco | 0:351a2fb21235 | 51 | * This method is called periodically by a task sequencer. |
Appalco | 0:351a2fb21235 | 52 | * @param period the period of the task sequencer, given in [s]. |
Appalco | 0:351a2fb21235 | 53 | * @return the status of this task, i.e. RUNNING or DONE. |
Appalco | 0:351a2fb21235 | 54 | */ |
Appalco | 0:351a2fb21235 | 55 | int TaskMoveToWaypoint::run(float period) { |
Appalco | 0:351a2fb21235 | 56 | |
Appalco | 0:351a2fb21235 | 57 | float translationalVelocity = 0.0f; |
Appalco | 0:351a2fb21235 | 58 | float rotationalVelocity = 0.0f; |
Appalco | 0:351a2fb21235 | 59 | |
Appalco | 0:351a2fb21235 | 60 | float x = controller.getX(); |
Appalco | 0:351a2fb21235 | 61 | float y = controller.getY(); |
Appalco | 0:351a2fb21235 | 62 | float alpha = controller.getAlpha(); |
Appalco | 0:351a2fb21235 | 63 | |
Appalco | 0:351a2fb21235 | 64 | float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y)); |
Appalco | 0:351a2fb21235 | 65 | float gamma = atan2(this->y-y, this->x-x)-alpha; |
Appalco | 0:351a2fb21235 | 66 | |
Appalco | 0:351a2fb21235 | 67 | while (gamma < -PI) gamma += 2.0f*PI; |
Appalco | 0:351a2fb21235 | 68 | while (gamma > PI) gamma -= 2.0f*PI; |
Appalco | 0:351a2fb21235 | 69 | |
Appalco | 0:351a2fb21235 | 70 | if (initialDistance == 0.0f) initialDistance = rho; |
Appalco | 0:351a2fb21235 | 71 | |
Appalco | 0:351a2fb21235 | 72 | translationalVelocity = velocity; |
Appalco | 0:351a2fb21235 | 73 | rotationalVelocity = (rho < initialDistance/2.0f) ? 0.0f : K*gamma; |
Appalco | 0:351a2fb21235 | 74 | |
Appalco | 0:351a2fb21235 | 75 | controller.setTranslationalVelocity(translationalVelocity); |
Appalco | 0:351a2fb21235 | 76 | controller.setRotationalVelocity(rotationalVelocity); |
Appalco | 0:351a2fb21235 | 77 | |
Appalco | 0:351a2fb21235 | 78 | return (rho < initialDistance/2.0f) && ((gamma < -PI/2.0f) || (gamma > PI/2.0f)) ? DONE : RUNNING; |
Appalco | 0:351a2fb21235 | 79 | } |
Appalco | 0:351a2fb21235 | 80 |