MotionController.hpp

#ifndef MOTIONCONTROLLER_HPP
#define MOTIONCONTROLLER_HPP

#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/Gyro.hpp>

#define WHEEL_RADIUS 0.02005146
#define MOTOR_MAX_SPEED 6.00
#define CELL_LENGTH 0.25

enum class MotionState {
        IDLE,
        MOVING,
        TURNING,
    };

class MotionController {
public:
    /**
     * @param robot_ptr The Webot Robot object
     * @param time_step Timestep (milliseconds)
     * @param x_ptr Pointer to the bot's x coordinate variable
     * @param y_ptr Pointer to the bot's y coordinate variable
     * @param dir Pointer to the bot's direction variable
     */
    MotionController(webots::Robot *robot_ptr, int time_step, int *x_ptr, int *y_ptr, int *dir);
    /**
     * @brief returns the current movement state (IDLE, MOVING or TURNING)
     * @return returns state as MotionController::MotionState enum
     */
    MotionState getState();
    /**
    * @brief Moves the robot forward by distance in meters and busy waits until movement is complete.
    * @param distance Distance to move in meters. Positive = forward, Negative = backward
    * @param brake Whether to use brake or not (Don't if planning to continue)
    * @param wait If true, the function doesn't return until the destination is reached
    */
    void moveDistance(double distance, bool brake = true, bool wait = true);
    /**
     * @brief Rotates the robot by angle in degrees and busy waits until movement is complete.
     * @param angle Angle to rotate in degrees. Positive = Clockwise(Right), Negative = Anti-Clockwise(Left)
     * @param wait If true, the function doesn't return until the target angle is reached
     */
    void rotateAngle(double angle, bool wait = true);
    /**
     * @brief If wait was set to false in any movement, update must be called in every iteration for the movement to stop at the desired location
     */
    void update();
    /**
     * @brief Stop both at any moment 
     */
    void stopMotors();

private:
    webots::Robot *robot;
    webots::Motor *leftMotor;
    webots::Motor *rightMotor;
    webots::PositionSensor *leftEncoder;
    webots::PositionSensor *rightEncoder;
    webots::Gyro *gyro;
    const int timestep;
    const double brakingDistance = 0.3; // Angle to start slowing down while turning
    const double *gyroLookupTable;
    bool useLinearBrakes;
    MotionState currentState = MotionState::IDLE;
    
    double wheelSpeed;
    double leftWheelTarget;
    double rightWheelTarget;
    double linearErrorMargin;
    double angleToTurn;

    int *p_x, *p_y, *p_d;

    void updateCoords(int cells);
    void updateDirection(int turns);
};

#endif