Config.h

#include <math.h>

// State machine
#define EXECUTION_RATE 20 // [Hz]
#define OVERALL_SPEED_GAIN 1
#define FOLLOW_WALL_ONLY_MODE false
#define FRONT_WALL_DISTANCE 0.3 // distance to wall in front of robot in order to get out of GO_STRAIGHT state
#define Y_GAIN_GO_STRAIGHT 0.05
#define MIN_DISTANCE_Y_GAIN 0.3
#define MIN_DISTANCE_DEAD_END 0.8
#define MOVE_ON true // set to false to turn off robot movements (for test purposes)

// Pledge
#define DELTA_CORNER_TAKEN 0.4 //[rad] the deviation between corner and pico to add corner count.
#define DELTA_ZERO_ANGLE 0.02 //[rad] the angle for the straight forward count

// Potential field
#define FRONT_SCALING_FACTOR 0.75
#define RANGE_FRONT_SCALING 0.25
#define MAX_RANGE 0.4
#define MIN_RANGE 0.01
#define DISTANCE_OFFSET 0.1
#define THETA_DEAD_ZONE 0.1

// Follow wall
#define WALL_FOLLOW_DISTANCE_SETPOINT 0.3
#define WALL_VISIBILITY_CIRCLE 0.4
#define MAX_Y_VALUE 0.6
#define X_SPEED_GAIN 0.75
#define DIRECTION false // true = R  (turn right and follow left wall)
                       // false = L
// Motion
typedef float Speeds[3];
#define X_SPEED_SATURATION 1
#define Y_SPEED_SATURATION 0.2
#define THETA_SPEED_SATURATION M_PI

// Corner detection
#define MINIMUM_CORNER_DISTANCE 0.1
typedef float LaserCoord[1000][3];
typedef int CornerSect[1000][2];
typedef float CornerInfo[1000][3];

// Dead end detection
#define DISTANCE_DOOR_REFERENCE_POINT 0 // distance from door to reference point in front of door

Environment Scan

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include "corner_detection.h"
#include <emc/io.h>
#include <emc/rate.h>

using namespace std;

void EnvironmentScan(emc::LaserData scan, LaserCoord laserCoordinates)
{
    float ScanLength = 0.0;
    double a = scan.angle_min;

    for(unsigned int i = 0; i < scan.ranges.size(); ++i)
    {
        if(scan.ranges[i] < 0.1 || scan.ranges[i] > 3)
        {
            ScanLength = 3;
        }
        else
        {
            ScanLength =scan.ranges[i];
        }
        double x = cos(a) * ScanLength;
        double y = sin(a) * ScanLength;

        laserCoordinates[i][0] = x;
        laserCoordinates[i][1] = y;
        laserCoordinates[i][2] = 0;

        a += scan.angle_increment;
    }
}