ObstacleDetectorWithServo.h

Go to the documentation of this file.

#pragma once

#include <optional>

#include "Image.h"
#include "Servo.h"
#include "mbed.h"

namespace nxpcup {

class ObstacleDetectorWithServo {
public:
    struct Config {
        PinName sensorPin; 
        Servo::Config servoConfig; 
        int thresholdDistance; 
        int moveFromObstacle; 
        float encoderAvoidDistance; 
        bool isDeactivate = false; 
        // 5 degree step is 36 image size...
        // 10 degree step is 18 image size...
        static constexpr int IMAGE_SIZE = 30;
        static constexpr int SERVO_MAX_RANGE_DEGREE = 180;
    };

    enum class AvoidingObstacle {
        no,
        onRightSide,
        onLeftSide
    };

    using ImageType = uint16_t;
    using ObstacleImage = Image<ImageType, Config::IMAGE_SIZE>;

    ObstacleDetectorWithServo(Config config)
        : m_sensor(config.sensorPin)
        , m_servo(config.servoConfig)
        , m_config(config){};

    int servoAngle(int index)
    {
        int servoAngleRange = m_servo.getMaxAngle() - m_servo.getMinAngle();
        auto angle = m_servo.getMinAngle() + servoAngleRange * index / m_image.size;
        return angle;
    }

    int servoPositionDegree() const
    {
        return m_servoPositionDegree;
    }

    int sensorValue() const
    {
        return m_sensorValue;
    }

    int error(float encoderDistance,
        int borderDetectorError,
        int leftBorder,
        int rightBorder)
    {
        update();
        check(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);
        ; // see just left line
    }

    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;
        }
    }

    int distanceThatTriggered() const
    {
        return m_distanceThatTriggered;
    }

    Config config() const
    {
        return m_config;
    }

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

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

private:
    int update()
    {
        // filtration - average
        m_sensorValue = 0;
        for (int i = 0; i < 10; ++i) {
            m_sensorValue += m_sensor.read_u16();
        }
        m_sensorValue /= 10;

        m_image[m_index] = m_sensorValue;

        if (m_direction == 1) {
            m_index++;
            if (m_index == (m_image.size - 1)) {
                m_direction = -1;
            }
        } else if (m_direction == -1) {
            m_index--;
            if (m_index == 0) {
                m_direction = 1;
            }
        }

        m_servoPositionDegree = servoAngle(m_index);
        m_servo.setAngle(m_servoPositionDegree);

        return m_sensorValue;
    }

    std::optional<int> checkObstacleAngle()
    {
        Image processed = m_image;
        int maxElementIndex = std::max_element(processed.begin(), processed.end())
            - processed.begin();

        int maxElementValue = processed[maxElementIndex];
        if (maxElementValue < m_config.thresholdDistance) {
            return {};
        }
        m_distanceThatTriggered = maxElementValue;
        return { servoAngle(maxElementIndex) };
    }

    void check(float encoderDistance)
    {
        if (m_avoidingObstacle == AvoidingObstacle::no) {
            auto obstacle = checkObstacleAngle();

            if (!obstacle) {
                return;
            }
            if (*obstacle < 90) {
                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_sensor;
    Servo m_servo;
    Config m_config;

    AvoidingObstacle m_avoidingObstacle = AvoidingObstacle::no;
    float m_encoderDistanceStart = 0;
    int m_sensorValue = 0;
    int m_servoPositionDegree = 90;
    int m_direction = 1; // -1 = left; 1 = right
    int m_index = 0;
    int m_distanceThatTriggered = 0;
    ObstacleImage m_image;
};

} // namespace nxpcup