motionwrapper.cpp

#include "motionwrapper.h"
#include <emc/io.h>
#include "odometry.h"
#include <iostream>
#include "laservision.h"
#include "my_odometry.h"
#include <ctime>
#include <cstdio>
using namespace std;

double vmax = 0.4; //max velocity
double rmax = 0.9; //max angular velocity
double mw_radmargin = 0.05; // make radius slightly bigger
double rotgain = 5; // align_error-->ang_vel feedback gain
double vygain = 5; // y_error --> y_vel feedback gain
double mw_gain = 5; // Gain used in turns for tangent./orthog. traject. error feedback
double ref;
double maxradius = 0.5; // turnradius
bool inTurn;
double inOpenSpace = 1; //open space wall distance treshold

my_od my_od_move; //ad_hoc world-fixed,robot-aligned odometry frame
my_od_frame frame_move;
//============================================================
bool mv_forward(emc::IO &io ){

    // in open space use different policy
    // but robustify for corridor situations
    if(lv_junction_data.d1l > inOpenSpace || lv_junction_data.d1r > inOpenSpace){
        if(lv_junction_data.d1l < 0.5){
            lv_yposerror = lv_junction_data.d1l;
            lv_yposerror = -lv_junction_data.d1l +0.4;
        }
        if(lv_junction_data.d1r < 0.5){
            lv_yposerror = lv_junction_data.d1r;
            lv_yposerror = -lv_junction_data.d1r +0.4;
        }
    }
    io.sendBaseReference(vmax,lv_yposerror*vygain,lv_misalign*rotgain);
}
//============================================================
//  X,Y odometry error feedback loop

double mw_xff; //feedforward x speed precalculated
double mw_xerror;
double mw_yerror;
double d;
double mw_vx;
double mw_vy;

bool mv_turnleft(emc::IO &io, emc::OdometryData &odom, double d_ ){

    //before starting turn do some bookkeeping
    if(inTurn != true){
        inTurn = true;
        ref = od_rotabs; // save abs rot, for reference
        d = d_+mw_radmargin;

        // cap max turning radius, make super small if open space
        if(d>maxradius)
            d = maxradius;
        //if(lv_junction_data.d1l > inOpenSpace || lv_junction_data.d1r > inOpenSpace)
            //d = 0.1;

        frame_move = my_od_reset_frame(odom); //stick robot-aligned frame origin to robot

        //Calculate x-feedforward that would yield perfect trajectory in perfect world
        mw_xff = d/rmax;
    }
    //calc x and y error in junction-fixed frame
    my_od_move = my_od_update(frame_move,odom);
    mw_xerror = my_od_move.x - d*sin(od_rotabs-ref);
    mw_yerror = my_od_move.y - (d-d*cos(od_rotabs-ref));

    //translate to tangential and orthogonal reference for x,y
    mw_vx = cos(od_rotabs-ref)*mw_xerror + sin(od_rotabs-ref)*mw_yerror;
    mw_vy = -sin(od_rotabs-ref)*mw_xerror + cos(od_rotabs-ref)*mw_yerror;

    //feedback into base reference
    io.sendBaseReference(-mw_vx*mw_gain + mw_xff,-mw_vy*mw_gain,rmax);
    if(fabs(od_rotabs - ref) > 3.141592/2){
        io.sendBaseReference(0,0,0);
        inTurn = false;
        return true;
        d=0;
    }
    else{
        return false;
    }
}


//============================================================
// X,Y odometry error feedback loop
bool mv_turnright(emc::IO &io, emc::OdometryData &odom, double d_ ){

    //before starting turn do some bookkeeping
    if(inTurn != true){
        inTurn = true;
        d = d_+mw_radmargin;

        // cap max turning radius, make super small if open space
        if(d>maxradius)
            d = maxradius;
        //if(lv_junction_data.d1l > inOpenSpace || lv_junction_data.d1r > inOpenSpace)
            //d = 0.1;

        ref = od_rotabs; // save abs rot, for reference
        frame_move = my_od_reset_frame(odom); //stick robot-aligned frame origin to robot

        //Calculate x-feedforward that would yield perfect trajectory in perfect world
        mw_xff = d/rmax;
    }
    //calc x and y error in junction-fixed frame
    my_od_move = my_od_update(frame_move,odom);
    mw_xerror = my_od_move.x + d*sin(od_rotabs-ref);
    mw_yerror = my_od_move.y + (d-d*cos(od_rotabs-ref));

    //translate to tangential and orthogonal reference for x,y
    mw_vx = cos(od_rotabs-ref)*mw_xerror + sin(od_rotabs-ref)*mw_yerror;
    mw_vy = -sin(od_rotabs-ref)*mw_xerror + cos(od_rotabs-ref)*mw_yerror;

    //feedback into base reference
    io.sendBaseReference(-mw_vx*mw_gain+ mw_xff,-mw_vy*mw_gain,-rmax);
    if(fabs(od_rotabs - ref) > 3.141592/2){
        io.sendBaseReference(0,0,0);
        inTurn = false;
        d = 0;
        return true;
    }
    else{
        return false;
    }
}

//============================================================
bool mv_turnaround(emc::IO &io){
    if(inTurn != true){
        inTurn = true;
        ref = od_rotabs; // save abs rot, for reference
    }
    io.sendBaseReference(0,0,-rmax);
    if((fabs(od_rotabs - ref) > 3.141592) || lv_junction_data.type==0 ){ //if 180 turned or good corridor is seen again
        io.sendBaseReference(0,0,0);
        inTurn = false;
        return true;
    }
    else{
        return false;
    }
}
//============================================================
bool mv_halt(emc::IO &io){
    io.sendBaseReference(0,0,0);
}
//============================================================
int mw_doortimer;
int mw_doortime = 4;
bool mw_waiting = false;
bool mv_requestOpendoor(emc::IO &io){
    if(mw_waiting == false ){
        mw_waiting = true;
        mw_doortimer = clock();
        io.sendBaseReference(0,0,0);
        io.sendOpendoorRequest();
    }
    else{
        if((clock()-mw_doortimer) / CLOCKS_PER_SEC <= mw_doortime ){
            io.sendBaseReference(0,0,0);
            io.sendOpendoorRequest();
            cout << (clock()-mw_doortimer) / CLOCKS_PER_SEC <<endl;
            return false;
        }
        else{
            mw_waiting = false;
            return true;
        }
    }
}

// Cap speed for simulation purposes
double mw_cap(double speed){

    if(fabs(speed) > vmax){
        if (speed>0){
            return vmax;
        } else
        {
            return -vmax;
        }

    }
    else{
        return speed;
    }
}

motionwrapper.h

#ifndef MOTIONWRAPPER_H
#define MOTIONWRAPPER_H
#include <emc/io.h>

#include <stdlib.h>
#include <math.h>

bool mv_halt(emc::IO &io);
bool mv_turnleft(emc::IO &io, emc::OdometryData &odom,double d);
bool mv_turnright(emc::IO &io, emc::OdometryData &odom,double d);
bool mv_forward(emc::IO &io);
bool mv_turnaround(emc::IO &io);
bool mv_requestOpendoor(emc::IO &io); //returns true when 5 seconds have past                     
double mw_cap(double speed);

extern bool mw_waiting; //reset in supervisor
extern double rotgain;
extern double vygain;
extern double mw_gain;
extern double vmax;
extern double rmax;

enum Move{ forward=0, leftdir=-1,rightdir=1, pending=2, turn180=3, door = 4 } ;
#endif // MOTIONWRAPPER_H