NXT Maze - left hand rule fast moving

Nice left hand rule moving implementation.
















Robot moves inside simple maze with the "left hand" rule. Just another simple task for basic training NXT robots. Implemented in Tomsk Polytechnic University robotics lab.

Used sensor: ultrasonic; HT EOPD or Lego LightSensor;

Algorithm of sharp turns:
Robot makes left turn while there is no wall leftside or until right motor reaches full 180 degrees rotation count. This helps to avoid fails with incorrect leftturn recognition.

NXC code:

/* Maze */
#include "Methods.h"

// Movement Constants
int CONST_FORWARD = 90;
int CONST_TURN90 = 90;
int CONST_TURN90_TIME = 150;
int CONST_TURN90_DEGREES = 210;
int CONST_TURN180_LEFT = 30;
int CONST_TURN180_RIGHT = 80;
int CONST_TURN180_DEGREES = 1000;
int CONST_TURN180_AFTERTIME = 500;
float CONST_KP = 0.1;
        //distance
int CONST_WALL_DESIRED_DISTANCE = 350;
int CONST_WALL_MAX_DISTANCE = 200;
int CONST_DEADLOCK_MAX_DISTANCE = 10;
//Sensors
int SENSOR_eopd = 0;
int SENSOR_light = 0;
int SENSOR_touch = 0;
int SENSOR_rgb = 0;
int SENSOR_ultra = 0;
        
task Sensors()  {
     while ( true )  {
           SENSOR_eopd = READ_eopd();
           SENSOR_ultra = READ_ultra();
     }
}

task Display()    {
     while ( true )  {
           ClearScreen();
           PRINT_info( "LEFT WALL: ", SENSOR_eopd, 1 );
           PRINT_info( "ULTRA: ", SENSOR_ultra, 3 );
           Wait( 100 );
     }
}

task Movement()  {
     while ( true )  {
           if ( SENSOR_ultra > CONST_DEADLOCK_MAX_DISTANCE )  {
                if ( SENSOR_eopd > CONST_WALL_MAX_DISTANCE &&
                     SENSOR_eopd < 1024 )
                {
                     float DEV = MOVE_along_wall(
                            CONST_KP,
                            CONST_WALL_DESIRED_DISTANCE,
                            SENSOR_eopd );
                     OnFwd ( OUT_C, CONST_FORWARD + DEV );
                     OnFwd ( OUT_B, CONST_FORWARD - DEV );
                }
                else      {
                     ResetRotationCount( OUT_B );
                     while ( MotorRotationCount( OUT_B ) < CONST_TURN180_DEGREES
                             && SENSOR_eopd < CONST_WALL_MAX_DISTANCE )  {
                           OnFwd( OUT_C, CONST_TURN180_LEFT );
                           OnFwd( OUT_B, CONST_TURN180_RIGHT );
                     }
                     if ( SENSOR_eopd < CONST_WALL_MAX_DISTANCE )  {
                           OnFwd( OUT_BC, CONST_FORWARD );
                     }
                }
           }
           else  {
                 MOVE_turn90_right( CONST_TURN90, CONST_TURN90_TIME );
           }
     }
}

task main() {
     ATTACH_eopd();
     ATTACH_ultra();
     Precedes ( Sensors, Movement, Display );
}