mrm-robot-maze.h

#pragma once
#include <mrm-action.h>
#include <mrm-robot.h>
 
enum Direction{UP, LEFT, DOWN, RIGHT, NOWHERE};
enum LedSign { MAZE_IMU_FOLLOW, MAZE_LED_PAUSE, MAZE_LED_PLAY, MAZE_WALL_DOWN_FOLLOW, MAZE_WALL_LEFT_FOLLOW, MAZE_WALL_RIGHT_FOLLOW, MAZE_WALL_UP_FOLLOW }; // For mrm-8x8 display.
enum WallStatus{ NO_WALL, WALL_UNKNOWN, WALL_WITH_VICTIM, WALL_WITHOUT_VICTIM};
enum TileSurvace{ SURFACE_BLACK, SURFACE_SILVER, SURFACE_UNKNOWN, SURFACE_WHITE};
 
 
class Tile {
 
    uint8_t _wall; // 1 byte stores statuses for all 2 walls enclosing the tile. Bit 7:6 - right, 5:4 - down, 3:2 - left, 1:0 - up.
    uint8_t _surface; // 1 byte stores surface and other data. Bit 7:6 - surface. The rest 6 bytes can be freely used. This feature is not in use yet.
    Tile* _chain; // Pointer to the next tile in chain.
 
public:
    static Tile* first; // Static member variable (one instance for the whole class), pointing to the first tile of the chain.
 
    Direction breadcrumb; // Stores direction of the previous tile, needed for Tremaux algorithm. This sequence is independent of the one made by _chain variable, which is only used for traversing the chain.
    int8_t x; // x coordinate of the tile. Increasing in RIGHT direction.
    int8_t y; // y coordinate of the tile. Increasing in UP direction.
 
    Tile(int8_t x, int8_t y, Direction breadcrumb); 
 
    Tile* next() { return _chain; } 
 
    WallStatus wallGet(Direction direction) { return (WallStatus)((_wall >> (direction * 2)) & 0b11); }
 
    void wallSet(Direction direction, WallStatus wallStatus) { _wall &= ~(0b11 << (direction * 2)); _wall |= (wallStatus << (direction * 2)); }
};
 
 
/* All the Action-classes have to be forward declared here (before RobotMaze) as RobotMaze declaration uses them. The other option would be
not to declare them here, but in that case Action-objects in RobotMaze will have to be declared as ActionBase class, forcing downcast later in code, if
derived functions are used.*/
class ActionDecide;
class ActionMap;
class ActionMove;
class ActionMoveAhead;
class ActionMoveTurn;
class ActionRescueMaze;
class ActionWallsTest;
 
class RobotMaze : public Robot {
    // Change these values to get optimal robot's behaviour.
    const uint16_t IMU_FOLLOW_STRENGTH = 5; // A bigger value increases feedback.
    const uint16_t MAZE_MAX_STEPS = 0xFFFF; // Can be used to stop the robot after a certain number of steps, for example for debugging purpose.
    const uint16_t MOVE_AHEAD_TIMEOUT_MS = 1300; // If no encoders available, the robot will stop after this time elapses, when moving ahead over 1 tile.
    const uint16_t NO_WALL_DISTANCE = 250; // In mm. When a sensor measures a value bigger than this one, it will declare there is no wall in front of it.
    const uint8_t TOP_SPEED = 127; // Can be used to slow down the robot. Maximum value is 127. Enter 0 to test the robot without motors working.
    const uint16_t WALL_FOLLOW_DISTANCE = 90; // Distance for wall following. Normally it should be (300 mm - 120 mm) / 2. 300 mm is tile's width and 120 mm is robot's width.
    const uint16_t WALL_FOLLOW_ERROR_ALLOWED = 40; // In mm. If 2 sensors measure distances that diverge in values more than this number, the robot will not follow that wall.
    const uint16_t WALL_FOLLOW_DISTANCE_ADJUSTMENT_STRENGTH = 1.1; // A bigger value will force the robot to correct distance to a wall more vigorously.
    const uint16_t WALL_FOLLOW_ROTATION_STRENGTH = 1.1; // A bigger value will force the robot to correct its alignment to a wall more vigorously.
 
    // Actions' declarations
    ActionDecide* actionDecide;
    ActionBase* actionGoStraightAhead;
    ActionMap* actionMap;
    ActionMove* actionMove;
    ActionMoveAhead* actionMoveAhead;
    ActionMoveTurn* actionMoveTurn;
    ActionRescueMaze* actionRescueMaze;
    ActionWallsTest* actionWallsTest;
 
    Direction directionCurrent; // Current robot's direction.
 
    float imuLastValid = 9999; // Last measured compass value. Important for moving ahead when no wall available.
 
    MotorGroupDifferential* motorGroup = NULL; // Class that conveys commands to motors.
 
    uint16_t stepCount; // Number of steps made.
 
    Tile* tileCurrent; // Current tile (robot's position).
 
public:
    RobotMaze(char name[] = (char*)"RCJ Maze");
 
    void bitmapsSet();
 
    void decide();
 
    uint16_t distance(Direction direction, bool firstCCW) {return mrm_lid_can_b->reading(2 * mToR(direction) + firstCCW); }
 
    void directionDisplay(Direction direction);
 
    void goAhead() {}
 
    void imuFollow();
 
    void loop();
 
    void map();
 
    void mazePrint();
 
    void move();
 
    void moveAhead();
 
    void moveTurn();
 
    /* Direction from maze's perspective to robot's perspective. Robot's front is TOP, robot's back is DOWN.
    @param directionAsSeenInMaze - direction from maze's prespective.
    @return direction from robot's perspective.
    */
    Direction mToR(Direction directionAsSeenInMaze) { int8_t d = directionAsSeenInMaze - directionCurrent; return (Direction) (d < 0 ? d + 4 : d); }
 
    void omniWheelsTest();
 
    void rescueMaze();
 
    /* Direction from robot's perspective to maze's perspective. Robot's front is TOP, robot's back is BACK.
    @param directionAsSeenByRobot - direction from robot's perspective.
    @return direction from maze's perspective.
    */
    Direction rToM(Direction directionAsSeenByRobot) { return (Direction)(directionCurrent + directionAsSeenByRobot % 4); }
 
    Tile* tileContaining(int8_t x, int8_t y);
 
    Direction wallClosest();
 
    void wallFollow(Direction wallDirection);
 
    void wallsDisplay();
 
    WallStatus wallGet(int8_t x, int8_t y, Direction direction);
 
    void wallDisplay(WallStatus wallStatus, Direction direction);
 
    void wallsTest();
 
    int8_t x(Direction direction);
 
    int8_t y(Direction direction);
};
 
 
class ActionDecide : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->decide(); }
public:
    ActionDecide(Robot* robot) : ActionBase(robot, "", "") {}
};
 
class ActionMap : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->map(); }
public:
    ActionMap(Robot* robot) : ActionBase(robot, "", "") {}
};
 
class ActionMove : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->move(); }
public:
    Direction direction; // Initial direction is stored to be recalled when using the action.
    uint32_t startMs; // Time is stored, for example to be used to determine if the movement's timeout elapsed.
    ActionMove(Robot* robot) : ActionBase(robot, "", "") {}
};
 
class ActionMoveAhead : public ActionMove {
    void perform() { ((RobotMaze*)_robot)->moveAhead(); }
public:
    ActionMoveAhead(Robot* robot) : ActionMove(robot) {}
};
 
class ActionMoveTurn : public ActionMove {
    void perform() { ((RobotMaze*)_robot)->moveTurn(); }
public:
    float endAngle; // End condition: target angle.
    float turnByCCW; // Number of degrees to turn CCW (counter-clockwise).
    ActionMoveTurn(Robot* robot) : ActionMove(robot) {}
};
 
class ActionRescueMaze : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->rescueMaze(); }
public:
    ActionRescueMaze(Robot* robot) : ActionBase(robot, "maz", "Rescue Maze", 1) {}
};
 
class ActionOmniWheelsTest : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->omniWheelsTest(); }
public:
    ActionOmniWheelsTest(Robot* robot) : ActionBase(robot, "omn", "Test omni wheels", 1) {}
};
 
class ActionWallsTest : public ActionBase {
    void perform() { ((RobotMaze*)_robot)->wallsTest(); }
public:
    ActionWallsTest(Robot* robot) : ActionBase(robot, "wlt", "Walls test", 1) {}
};