ObstacleDetector.h

Go to the documentation of this file.

#pragma once

#include "mbed.h"

namespace nxpcup {

class ObstacleDetector {
public:
    struct Config {
        PinName leftSensorPin; 
        PinName rightSensorPin; 
        int thresholdDistance; 
        int moveFromObstacle; 
        float encoderAvoidDistance; 
        bool isDeactivate = false; 
    };

    enum class AvoidingObstacle {
        no,
        onRightSide,
        onLeftSide
    };

    ObstacleDetector(Config& config)
        : m_leftSensor(config.leftSensorPin)
        , m_rightSensor(config.rightSensorPin)
        , m_config(config)
    {
    }

    int error(float encoderDistance,
        int borderDetectorError,
        int leftBorder,
        int rightBorder)
    {
        updateSensorValue();
        checkObstacle(encoderDistance);

        if (m_config.isDeactivate) {
            return borderDetectorError;
        }

        if (m_avoidingObstacle == AvoidingObstacle::no) {
            return borderDetectorError;
        }
        if (m_avoidingObstacle == AvoidingObstacle::onLeftSide) {
            return leftBorder - (64 - m_config.moveFromObstacle); // see just right line;
        }
        // obstacle on the right side
        return rightBorder - (64 + m_config.moveFromObstacle);
    }

    int leftSensorValue() const
    {
        return m_leftSensorValue;
    }

    int rightSensorValue() const
    {
        return m_rightSensorValue;
    }

    AvoidingObstacle avoidingObstacle() const
    {
        return m_avoidingObstacle;
    }

    int avoidingObstacleInteger() const
    {
        switch (m_avoidingObstacle) {
        case AvoidingObstacle::no:
        default:
            return 0;
        case AvoidingObstacle::onLeftSide:
            return -1;
        case AvoidingObstacle::onRightSide:
            return 1;
        }
    }

    Config config() const
    {
        return m_config;
    }

    void setConfig(Config& config)
    {
        m_config = config;
        reset();
    }

    void reset()
    {
        m_encoderDistanceStart = 0;
        m_avoidingObstacle = AvoidingObstacle::no;
    }

private:
    void updateSensorValue()
    {
        m_leftSensorValue = m_leftSensor.read_u16();
        m_rightSensorValue = m_rightSensor.read_u16();
    }

    void checkObstacle(float encoderDistance)
    {
        if (m_avoidingObstacle == AvoidingObstacle::no) {
            if (m_leftSensorValue < m_config.thresholdDistance
                && m_rightSensorValue < m_config.thresholdDistance) {
                return;
            }
            if (m_leftSensorValue > m_config.thresholdDistance) {
                m_avoidingObstacle = AvoidingObstacle::onLeftSide; // see just right line;
            } else {
                m_avoidingObstacle = AvoidingObstacle::onRightSide; // see just left line
            }
            m_encoderDistanceStart = encoderDistance;
        } else {
            if (m_encoderDistanceStart + m_config.encoderAvoidDistance < encoderDistance) {
                m_avoidingObstacle = AvoidingObstacle::no;
            }
        }
    }

    AnalogIn m_leftSensor;
    AnalogIn m_rightSensor;
    Config m_config;

    AvoidingObstacle m_avoidingObstacle = AvoidingObstacle::no;
    int m_leftSensorValue = 0;
    int m_rightSensorValue = 0;
    float m_encoderDistanceStart = 0;
};

} // namespace nxpcup