main

main.cpp

// emc-framework
#include <emc/io.h>
#include <emc/rate.h>
#include <iostream>
#include <stdio.h>
#include <time.h>
 
// subsystems
#include "movement.h"
#include "detection.h"
#include "strategy.h"

int main() 
{
// create instances of objects
    // from emc-framework
    emc::IO io;
    emc::Rate r(40);
    emc::LaserData scan;
    emc::OdometryData odom, odom_ini; // measured and initial odometry data.
 
    Point CurrentLocation; // current location for cout output in main
    time_t time1,time2;    // time objects used for time keeping in for door request
 
    // from subsystems (are 'send-by-reference' in the functions (e.g strategy(&mov)))
    Movement mov;
    Detection det;
    Strategy strat;
 
    // INITIAL ODOMETRY READING:=========================================//
    while(true){ //keep looping untill initial odometry data is measured
        // If odometry data is measured:
        if(io.ok() && io.readOdometryData(odom_ini)) {
 
            //Reset coordinate system using initial odometry data:
            det.ResetOdomOrigin(odom_ini);
 
            std::cout << "Initial odometry offset is measured." << std::endl;
 
            //break the while loop and proceed with the programm.
            break;
        }
        r.sleep(); //prevents the loop from running very fast.
    }
    //===================================================================//
 
    // MAIN LOOP:
    while(io.ok()){
 
        if(io.readOdometryData(odom) && io.readLaserData(scan)){
 
            // trigger the coordinator/composer of detection
            det.detection(io, scan, odom);
 
            // if situation is recognized or strategy needs to be triggered by itself
            if(det.monitor() == true || strat.monitor() == 1) {
 
                // trigger the coordinator/composer of strategy
                strat.strategy(io, scan, odom, det, mov);
 
                // if stratey orders an door-open-request:
                if(strat.monitor() == 2) {
 
                    // Run Movement to set velocity to zero:
                    mov.movement(io, scan, det, strat);
 
                    // Request door open:
                    io.sendOpendoorRequest();
                    std::cout << "Door request..." << std::endl;
 
                    // Wait five seconds:
                    time1 = time(0); //save time.
                    time2 = time(0);
                    while (5 > difftime(time2,time1)) {
                        time2 = time(0);
//                        std::cout << "waiting..." << std::endl;
                    }
 
                    //Run detection again to check if the door has opened:
                    det.detection(io, scan, odom);
 
                    //Run strategy again to act acordingly:
                    strat.strategy(io, scan, odom, det, mov);
                }
            }
 
            // trigger the coordinator/composer of movement
            mov.movement(io, scan, det, strat);
 
            //Print current location:
            //CurrentLocation = det.getCurrentLocation();
            //std::cout << "robot pos: (" << CurrentLocation.x << ',' << CurrentLocation.y << ',' << CurrentLocation.a << ')' << std::endl;
         }
        r.sleep();
    }
    return 0;
}