RobotPosition.c

void robotPosition::initialDistance(emc::IO &io, emc::LaserData &scan){
    // Measure how how much distance is left in this corridor
    StoredDistance = takeMeasurement(io, scan);
}

void robotPosition::readyForCorner(emc::IO& io, emc::LaserData& scan){
    // Reset the Variables which mark whether a Special Situation ended and if the
    //  robotPosition-measurement has been taken.
    MeasurementAfterTaken = false;
    CornerFininshed = false;
    UTurnDetected = false;
    // Before a corner the following steps have to be exectued in this particular order:
    // * The robot has to remeasure its position with respect to the wall
    double LeftInCorridor = takeMeasurement(io, scan);
    // To find the new position of the PICO Robot this LeftInCorridor has to be subtracted
    //  from the distance measured (StoredDistance) when at the start of this corridor
    // * The PICO Robot position is updated to the new coordinate
    updatePICOPositionBefore(StoredDistance-LeftInCorridor);
    StoredDistance = LeftInCorridor;
    // * For the Landmark which is being mapped, one coordinate can already be saved
    //    depending on the orientation of the robot, this will help in making the estimate
    //    of the LandMark
    firstCoordinateLandMark();
    // * Then the Odometry data can be saved as such that after the turn we can determine
    //    the position relative to the start again.
    io.readOdometryData(SavedOdometry);
    // * An Estimate has to be given on the location of the special situation which is
    //    begin detected. This is for the Solving Algorithm an indication whether we have
    //    already visited that node (Return value of Function)
    // For this we only have to correct for the current offset to the node, here it is assumed
    //  that the position of the LandMark with respect to the robot is almost constant upon
    //  detection. Assumed is that it detects the Landmark a constant DIST_BEFORE_LANDMARK before
    //  the midpoint of the junction. Note that at this point we only require an estimate of the
    //  landmark
    estimateLandMarkPosition();
}

void robotPosition::CorneringFinished(choices Direction){
    if(UTurnDetected){
        Direction.F = 0;
        Direction.R = 0;
        Direction.B = 1;
        Direction.L = 0;
    }
    // Updating the PICO robot's position
    updatePICOPositionAfter(Direction, CurrentOdometry);
    // Rest is only Map the Junction! and
    secondCoordinateLandMark(CurrentOdometry);
    CornerFininshed = true;
}

bool robotPosition::Cornering(choices Direction, emc::IO& io, emc::LaserData& scan){
    // In this function we see whether the robot has turned 90 degrees, if so a new
    //  LRF measurement is made to determine the position of the robot relative to the next
    //  junction
    io.readOdometryData(CurrentOdometry);
    if(UTurnDetected){
        Direction.F = 0;
        Direction.R = 0;
        Direction.B = 1;
        Direction.L = 0;
    }
    // Second, check whether the robot has turned 90 degrees
    if(Direction.R==1 && !MeasurementAfterTaken){ // Which means turn Right   Clockwise Rotations are in Odometry Negative!
        if((CurrentOdometry.a - SavedOdometry.a < -PI/2 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > -PI/2 - ROT_DEV)||\
            (CurrentOdometry.a - SavedOdometry.a < 3*PI/2 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > 3*PI/2 - ROT_DEV)){
            // Turn is finished, make a new measurement now
            StoredDistance = takeMeasurement(io,scan);
            MeasurementAfterTaken = true;
            return false;
        }
    }
    if(Direction.L==1 && !MeasurementAfterTaken){ // Which means turn Left   Counter-Clockwise Rotations are in Odometry Positive!
        if((CurrentOdometry.a - SavedOdometry.a < PI/2 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > PI/2 - ROT_DEV)||\
            (CurrentOdometry.a - SavedOdometry.a < -3*PI/2 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > -3*PI/2 - ROT_DEV)){
            // Turn is finished, make a new measurement now
            StoredDistance = takeMeasurement(io,scan);
            MeasurementAfterTaken = true;
            return false;
        }
    }
    if(Direction.B==1 && !MeasurementAfterTaken){ // Which means U-turn
        if((CurrentOdometry.a - SavedOdometry.a < -PI + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > -PI - ROT_DEV)||\
            (CurrentOdometry.a - SavedOdometry.a < +PI + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > +PI - ROT_DEV)){
            // Turn is finished, make a new measurement now
            StoredDistance = takeMeasurement(io,scan);
            MeasurementAfterTaken = true;
            return false;
        }
    }
    if(Direction.R==1 && MeasurementAfterTaken){
       if((CurrentOdometry.a - SavedOdometry.a < -3*PI/4 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > -3*PI/4 - ROT_DEV)||\
                (CurrentOdometry.a - SavedOdometry.a < 5*PI/4 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > 5*PI/4 - ROT_DEV)){
                UTurnDetected = true;
        }
    }
    if(Direction.L==1 && MeasurementAfterTaken){
        if((CurrentOdometry.a - SavedOdometry.a < 3*PI/4 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > 3*PI/4 - ROT_DEV)||\
                (CurrentOdometry.a - SavedOdometry.a < -5*PI/4 + ROT_DEV && CurrentOdometry.a - SavedOdometry.a > -5*PI/4 - ROT_DEV)){
                UTurnDetected = true;
        }
    }
    return MeasurementAfterTaken;
}

RobotPosition.h

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

// ########## Robot Position ##########
#define DIST_BEFORE_LANDMARK 1.2
#define ROT_DEV 0.1
#define SPEC_SIT_RADIUS 0.3
#define PI 3.14

enum Orient { North, West, South, East };

struct globalPosition{
    double PositionX;
    double PositionY;
    Orient Orientation;
} ;

struct choices{
        int F;
        int R;
        int B;
        int L;
};

class robotPosition {
    unsigned int NrPointsPerSide;
    globalPosition PICOPosition;
    globalPosition CurrentLandMark;
    double StoredDistance;
    emc::OdometryData SavedOdometry;
    emc::OdometryData CurrentOdometry;
    // Private Functions:
    //  --- takeMeasurement
    double takeMeasurement(emc::IO &io, emc::LaserData &scan);
    //  --- updatePICOPositionAfter
    void updatePICOPositionAfter(choices Direction, emc::OdometryData& CurrentOdo);
    //  --- secondCoordinateLandMark
    globalPosition secondCoordinateLandMark(emc::OdometryData& CurrentOdo);
    //  --- updatePICOPositionBefore
    void updatePICOPositionBefore(double DistanceFromLastLandmark);
    //  --- estimateLandMarkPosition
    void estimateLandMarkPosition();
    //  --- firstCoordinateLandMark
    void firstCoordinateLandMark();
public:
    bool MeasurementAfterTaken;
    bool CornerFininshed;
    bool UTurnDetected;
    // --- Constructor
    robotPosition(unsigned int NrPoints);
    //  --- initialDistance
    void initialDistance(emc::IO &io, emc::LaserData &scan);
    // --- readyForCorner
    void readyForCorner(emc::IO& io, emc::LaserData& scan);
    // --- Cornering
    bool Cornering(choices Direction, emc::IO& io, emc::LaserData& scan);
    // --- presentPICOPosition
    void presentPICOPosition();
    // --- presentLandMarkPosition
    void presentLandMarkPosition();
    // --- presentAngle
    void presentAngle();
    // --- getGlobalXPos
    double getGlobalXPos();
    // --- getGlobalYPos
    double getGlobalYPos();
    // --- CorneringFinished
    void CorneringFinished(choices Direction);
    // --- Destructor
    ~robotPosition();
};

#endif