strategy.cpp

strategy.cpp

#include "strategy.h"

// forward declared dependencies
//===================================
#include "movement.h"
#include "detection.h"

// constructors
//===================================
Strategy::Strategy() {
    status = 0;     		// strategy is not triggered
    movtype = 1;    		// homing is used
    DoorFound = false;		// Initially, the door has not yet been found.
    choicel = 0; 			// choicel = -1 if choice left was unsuccessfull
    choicer = 0; 			// choice right
    choices = 0; 			// choice straight
    inf = 50;   			//Representing inf.distance (not an option)
    first_run = false;
    command = 0;
}

// getters
//===================================
std::vector<Point> Strategy::getTrajectory() {
    return trajectory;
}

Point Strategy::getPoint() {
    return goal;
}

int Strategy::getMovementType() {
    return movtype;  // return the used movement type (p2p,homing,oneeighty)
}

int Strategy::getHomingType() {
    return command; // return the direction which is used for homing (left,right,strait)
}

// monitors
//===================================
int Strategy::monitor() {
    return status; // return strategy status
}

// configurators
//===================================
void Strategy::configurator() {
}

// coordinator/composer
//===================================
void Strategy::strategy(emc::IO &io, emc::LaserData scan, emc::OdometryData odom, Detection &det, Movement &mov) {
/* Coordinator of the strategy subsystem.
 *
 * Used getters and setters from other subsystems:
 * - Point = det.getCurrentLocation():  Gives the current location of the robot
 * - det.ResetOdomOrigin(Point):        Is used to reset the origin of the used axis system
 * - situation = det.getSituation():    Returns information about the current situation (in situation struct):
 * - bool det.monitor():                Inticates if a situation is recognized;
 *
 */

    float action_end_angle; //setpoint angle that needs to be reached during cornering

    //COMMUNICATION ===========================================//
    A = det.getCurrentLocation();

    // Determine angle after which action is completed:
    if (movtype == 2) { // a one-eighty trun is being performed
        action_end_angle = 175*PI/180; // do not make 180, this will force to robot to move exactly 180, without tollerances!
    }
    else {              // a 90 degree turn is being performed
        action_end_angle = 89*PI/180;
    }

    // MAKE DESICION ============================================================//
    // Based on the status of the different subsystems: determine what to do:
    if(status == 0){ // A new situation is encountered!

        //Communication:
        status = 1;             // set status to 1, cause strategy has been activated;
        det.ResetOdomOrigin(odom); // This resets the origin of the current coordinate system;
        detection_situation = det.getSituation(); // receive integer with current situation from detection;

        //if situation is a dead end: ask for door open request in main:
        if(detection_situation.type == 2){
            if(DoorFound == false) { //This is the first time a door is encountered
                status = 2; // Ask for open door request
                movtype = 3; // stop robot movement
            }
            else{ //the door has already been found: turn one-eigty
                movtype = 2; // set movement to turn 180 degrees
                status = 1;
                choice = 3;
            }
        }        

    else if (status == 1) {//The action resulting from a previously encountered situation still has to be finished.

        if(std::abs(A.a) > action_end_angle){ //The robot has turned enough, the action has ended:
            status  = 0;            // strategy turned off in the main loop
            movtype = 1;            // activate homing in movement subsystem
            command = 0;            // homing: staight ahead.
            history.push_back(choice);
        }
        else if((std::abs(A.x+A.y)>(dinstancetosit+0.2) && command == 0) || (std::abs(A.x+A.y)>(dinstancetosit+0.2) && command == 3))
        {
            status  = 0;            // strategy turned off in the main loop
            movtype = 1;            // activate homing in movement subsystem
            command = 0;            // homing: staight ahead.
            history.push_back(choice);
        }
        else { //situation is not yet finished, do nothing, motion will proceed

        }
    }

    else if (status == 2) {// An door open request has been placed previously

        detection_situation = det.getSituation(); // receive integer with current situation from detection;

        if(detection_situation.type == 2){ // The situation is still a dead end.
            movtype = 2; // set movement to turn 180 degrees
            status = 1;
			
            std::cout << "Door has not openend. :(" << std::endl;
        }

        else{ // if another situation is encountered, act acoordingly
            movtype = 1;
            command = 0;
            status = 0;

            std::cout << "Door has openend! :D" << std::endl;
        }
    }
    std::cout << "strategy status:" << status << std::endl;
}

// computational entities
//===================================