mazesolver.cpp

#include "mazesolver.h"

//SuperVisor Loop ======================================================
void mainloop(){

    //---Updated every loop-------------------------------------
    io.readLaserData(scan);
    io.readOdometryData(odom);
    lv_updateminimap(scan,lv_updatecompass(scan)); //update laservision
    lv_updatecompass(scan); //update laservision
    lv_updateypositionerror(); //update laservision
    lv_junction(); //update laservision
    od_logAbsoluteOdometry(odom); //log odometry
    my_od_abs = my_od_update(frame_abs,odom); //update user frames

    //---Checked every loop--------------------------------------
    jt = lv_junction_data.type;
    if((jt != 0 && ms_inTurn == false) ){ //detected something and not turning
        if(od_check_distkeeper() || (jt==7 && move != door  && move != turn180 )  ){ // if away from last branch or deadend
            move = pending; //Stop robot if branch is detected
        }
    }

    //---Decision making --------------------------------------
    if(move == pending){ //translate possible path to action taken
        ps.appendNodeRHR(odom,lv_junction_data); //mark junction
        //ps.plotGraph();
        move = ps.getNextMoveRHR(lv_junction_data); //get decision
    }

    //---Motion skills switch case-----------------------------
    switch(move){
    case pending:
        mv_halt(io);
        break;
    case leftdir:
        cout << "move: left" << endl;
        ms_inTurn = true;
        if(mv_turnleft(io, odom,lv_junction_data.d3)){
            move = forward;
            ms_inTurn = false;
        }
        od_set_distkeeper(0.2); //avoid redetection
        break;
    case rightdir:
        cout << "move: right" << endl;
        ms_inTurn = true;
        if(mv_turnright(io, odom,lv_junction_data.d2)){
            move = forward;
            ms_inTurn = false;
        }
        od_set_distkeeper(0.2);//avoid redetection
        break;
    case forward:
        cout << "move: forward" << endl;
        mv_forward(io);
        break;
    case turn180:
        cout << "move: turn180" << endl;
        if(mv_turnaround(io)){
            move = forward;
        }
        od_set_distkeeper(0.1);//avoid redetection
        break;
    case door:
        cout << "move: door" << endl;
        if(lv_junction_data.type==7 && mv_requestOpendoor(io)){

            move = turn180;
            mw_waiting = false;
        }
        if(lv_junction_data.type==0){
            move = forward;
            mw_waiting = false;

        }
        od_set_distkeeper(0.1);
        break;

    }
}

//Initialisation routine ================================================
void initialize(){
    while(!io.ok()){ //wait for io to be initialized
    }
    od_set_distkeeper(0.1);
    io.readOdometryData(odom);
    frame_abs = my_od_reset_frame(odom);
}

// start ================================================================
// startpoint of program, initialize and call mainloop()
int main()
{
    initialize();

    //askUser();
    //Do routine unless <error> or <finished> event is thrown
    while(status == busy){
        r.sleep();
        mainloop();
    }
    return 0;
}

//ask user for settings to start program with
char response;
void askUser(){
    //want to change anything?
    cout << "Change robot settings? (y=yes, n=no)\n>";
    cin >> response;
    if(response=='y')
    {
        string input;
        //============================================================================
        cout << "Set angle missalign gain (int) (default 5)" <<endl;
        getline(cin,input);
        stringstream(input) >> rotgain;

        std::cin.ignore(std::numeric_limits<std::streamsize>::max(),'\n');

        //============================================================================
        cout << "Set lateral error gain (int) (default 5)" <<endl;
        getline(cin,input);
        stringstream(input) >> vygain;

        //============================================================================
        cout << "Set turn trajectory correction gain (int) (default 5)" <<endl;
        getline(cin,input);
        stringstream(input) >> mw_gain;

        //============================================================================
        cout << "Set max rotation speed (double) (default 0.9)" <<endl;
        cin >> rmax;

        //============================================================================
        cout << "Set max foreard speed (double) (default 0.4)" <<endl;
        cin >> vmax;
        //============================================================================

        std::cin.ignore(std::numeric_limits<std::streamsize>::max(),'\n');
        //============================================================================
        cout << "Set supervisor loop rate (int) (default 15)" <<endl;

        getline(cin,input);
        int setrate;
        stringstream(input) >> setrate;
        r = emc::Rate(setrate);
    }
}

mazesolver.h

#ifndef MAZESOLVER
#define MAZESOLVER

#include <iostream>
#include <emc/io.h>
#include <emc/rate.h>
#include "laservision.h"
#include "odometry.h"
#include "motionwrapper.h"
#include <sstream>
#include "pathsolver.h"
#include "my_odometry.h"
#include <string>
#include "worldmodel.h"

using namespace std;
enum Status  { busy, error, finished } ;
void askUser();

static const int rate = 15;
pathsolver ps;
emc::IO io;
emc::Rate r(rate);
emc::LaserData scan;
emc::OdometryData odom;
Move move = forward;
Status status = busy;
my_od my_od_abs;
my_od_frame frame_abs;
using namespace std;
bool ms_inTurn = false;
int jt;

#endif // MAZESOLVER