Commit 6a2ed389 authored by Thibaut Perami's avatar Thibaut Perami
Browse files

WIP Async tasks

parent fc679fc0
......@@ -29,6 +29,10 @@ namespace Mobility {
/// execute an non-blocking move
void async();
enum State{RUNNING, STOPPED, FAILED};
/// Return the current state of movement.
State check();
static bool ignoreRadar;
private:
static bool radarState;
......
......@@ -50,6 +50,41 @@ void DirectGoTo::rearm(Vector2 dest){
mDest = dest;
}
NBDirectGoTo::NBDirectGoTo(string name,Vector2 dest,deque<Task*> newstack,
Distance epsilon,Angle alpha)
: Task(move(name)),mDest(dest),mEpsilon(epsilon),mAlpha(alpha),mNewStack(newstack){
}
bool NBDirectGoTo::step(){
Vector2 movement = mDest - Robot::sPosition;
Angle aMov = movement.orientation() - Robot::sOrientation;
info << "DGT going to" << mDest << "from" << Robot::sPosition << "\n";
debug << "DGT movement : " << movement << "and rotation " << aMov << "\n";
if(aMov > M_PI) aMov -= 2*M_PI;
if(aMov < -M_PI) aMov += 2*M_PI;
if(movement.Norm() < mEpsilon){
mDone = true;
return false;
}
if (std::abs(aMov) > mAlpha){
MotionTask mt(Mobility::Motion(0,aMov),"DirectGoTo Rotation");
mt.execute();
return true;
}
if(movement.Norm() > mEpsilon){
MotionTask mt(Mobility::Motion(movement.Norm(),0),"DirectGoTo Translation");
mt.execute();
}
mDone = true;
return false;
}
void DirectGoTo::rearm(Vector2 dest){
mDone = false;
mDest = dest;
}
/* ____ _ _____ _ _
/ ___|_ __ __ _| |__ | ___| __ ___ _ __ ___ | | | | ___ _ __ ___
| | _| '__/ _` | '_ \| |_ | '__/ _ \| '_ ` _ \| |_| |/ _ \ '__/ _ \
......
......@@ -2,6 +2,7 @@
#include <string>
#include <vector>
#include <deque>
#include "vector.h"
#include "graph.h"
#include "motion.h"
......@@ -21,11 +22,18 @@ namespace Behavior{
/// return true when task has failed
virtual bool failed() = 0;
// new stack to setup if failed
virtual std::deque<Task*> newStack(){
return {};
}
void execute(){
while(step());
}
bool done() {return mDone;}
std::string name(){ return mName;}
protected:
std::string mName;
bool mDone;
......@@ -44,6 +52,28 @@ namespace Behavior{
};
//Non-blocking version (fail immediately on blocking)
class NBDirectGoTo : public Task{
Vector2 mDest;
Distance mEpsilon;
Angle mAlpha;
std::deque<Task*> mNewStack;
bool mFailed = false;
public:
/// go directly to dest with precision epsilon in Distance and Alpha in angle.
NBDirectGoTo(std::string name,Vector2 dest,std::deque<Task*> newstack,
Distance epsilon = 0.1_cm,Angle alpha = 0.03);
void rearm(Vector2 dest);
bool step();
bool failed(){return false;}
std::deque<Task*> newStack(){
return mNewStack;
}
};
class Back : public Task{
Distance mDist;
public:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment