WallBot – NXC Version

Here is an NXC version of the HiTechnic WallBot. This program is essentially the same as the NXT-G program from the previous post except it does add one small improvement. This version displays on the NXT screen information such as the raw EOPD value, the processed steering value, and the power that is being used for the two motors.

Essentials:
Building Instructions for the HiTechnic Trike Base
Instructions for adding sensors to make the WallBot
The program: HTWallBotNXC.nxc

This NXC version of the WallBot is very close in function to the NXT-G program so you can use this as an example of how a NXT-G program can be converted to NXC.

If you hold the robot off the ground while the program is running, it is easy to see on the screen how the program responds to different conditions. For example, if you hold the robot close to a wall you will see that the left motor will slow down so that the robot will try to steer away from the wall. Similarly, if you hold the robot slightly away from the wall, for example 10cm away, then you will see that the robot will slow down the right motor to try to steer back towards the wall. If you take the robot completely away from the wall you will see the robot first proceed straight for a short distance and then start steering gradually to the right in an attempt to get around the corner.

Here is the entire program broken down into parts. After each part I will explain what that part of the program does.

//=====================================================================
// HiTechnic WallBot
//
// NXC version of the WallBot featured in the blog article
// www.hitechnic.com/blog/eopd-sensor/wallbot/
//
// The WallBot is a HiTechnic Trike Base equipped with an EOPD sensor
// and an Ultrasonic sensor.  The EOPD Sensor is mounted in the front
// right corner pointing at an angle forward and to the right.  The
// Ultrasonic sensor is below the EOPD facing forward.  See the blog
// article for detailed instructions.
//
// The EOPD Sensor is used to track the wall and the Ultrasonic sensor
// is used to detect inside corners where the robot needs to react and
// turn sharper than relying on the EOPD sensor alone.

// Input and Output Ports
#define RIGHT_MOTOR OUT_B
#define LEFT_MOTOR  OUT_C

#define EOPD        IN_3
#define US          IN_4

// Constants
#define EOPD_TARGET          100    // Target raw value for tracking
#define EOPD_MINWALL         30     // Min to determine wall is present

#define STEER_MAX_LEFT       -180   // Max left steer value when EOPD
                                    // very close to wall
#define STEER_INSIDE_CORNER  -100   // Forced left turn when Ultrasonic
                                    // sensor sees the wall
#define STEER_OUTSIDE_CORNER 70     // Steer value for outside corner.
                                    // Not too sharp

#define MRC_PAST_WALL        360    // Degrees robot should pass wall

#define US_THRESHOLD         20     // Ultrasonic threshold for turning

The code above contains a comment header as well as the constants used in the program. Putting constants in one place near the beginning of the program is a good programming practice. This makes it easy to change the values or to adapt the code for a different robot that might use, for example, different sensor or motor ports.

//---------------------------------------------------------------------
// int LimitPower(int power)
//
// Helper function to limit a power value to a range of -100 to 100
//
int LimitPower(int p)
{
  if (p > 100)
    return 100;
  else if (p < -100)
    return -100;
  return p;
}

This is simply a helper function that I will use below to make sure that the power for the motor is limited to a value from -100 to 100. In NXT-G it is okay to pass power values that are out of this range because it will automatically limit the value, in NXC you have to make sure the value is within the correct range or you can get unpredictable results.

task main()
{
  int eopd;
  int us;
  int pwrLeft, pwrRight;
  long mrcRightLastSeeWall = 0;
  int iSteer;

  SetSensorLowspeed(US);

  // The EOPD Sensor is a analog sensor like the LEGO Light sensor
  SetSensorType(EOPD, SENSOR_TYPE_LIGHT_ACTIVE);     // Set long range
  SetSensorMode(EOPD, SENSOR_MODE_RAW);

  TextOut(0, LCD_LINE1, "HiTechnic");
  TextOut(6*3, LCD_LINE2, "WallBot");

  TextOut(0, LCD_LINE7, "Right Power Left");

This is the beginning of the main program. First it declares local variables that will be used in the program, then it initializes the sensors, and finally displays on the screen the static text that will be there the whole time the program is running.

The LEGO Ultrasonic sensor is a digital I2C sensor and needs the SetSensorLowspeed command to tell the NXT that it will use I2C to communicate with the sensor. The EOPD sensor is an analog sensor, like the LEGO Light Sensor. The SetSensorType for the Light Sensor works perfectly for setting up the EOPD sensor as well. Setting it to Active puts it into long range mode, which is the mode most suitable for the WallBot.

The TextOut commands put text on the NXT screen that will stay there the whole time the program is running. The dynamic text will be put in further down in the program.

  while(true) {
    // Read EOPD Sensor
    eopd = 1023-SensorRaw(EOPD);

    TextOut(0, LCD_LINE4, "eopd:     ");
    NumOut(6*5, LCD_LINE4, eopd);

    iSteer = EOPD_TARGET - eopd;

Here is the beginning of the main control loop of the program. while (true) is simply a way to create an infinite loop in NXC. Since true is always true, this will loop forever.

The SensorRaw command returns a value that is from 0-1023. This value is reversed from the Raw value that you get out of the NXT-G block. So if the NXT-G would give you a raw value of 0, the SensorRaw function in NXC returns 1023. If NXT-G were to give you 1023, SensorRaw gives you 0. To make them the same, we need 1023-SensorRaw(EOPD).

    if (eopd < EOPD_MINWALL) {
      // eopd value is less then the minimum wall value.  This is an
      // outside corner and robot needs to take care not to run into
      // the corner with the right wheel.
      if (MotorRotationCount(RIGHT_MOTOR)
            < (mrcRightLastSeeWall + MRC_PAST_WALL)) {
        // Robot is too close to turn-in without hitting the corner
        iSteer = 0;
      } else {
        // Robot has now past the point where it is safe to turn right
        iSteer = STEER_OUTSIDE_CORNER;
      }
    } else {
      // Store motor encoder position that wall is seen
      mrcRightLastSeeWall = MotorRotationCount(RIGHT_MOTOR);

      if (iSteer < STEER_MAX_LEFT) {
        // Prevent too sharp a left turn that can cause back of robot
        // to hit wall.
        iSteer = STEER_MAX_LEFT;
      } else {
        us = SensorUS(US);
        if ((us < US_THRESHOLD) && (iSteer > STEER_INSIDE_CORNER))
          // If Ultrasonic sensor detects inside corner, force left
          // turn.  The && expression above makes sure that EOPD sensor
          // can force a sharper turn if necessary.
          iSteer = STEER_INSIDE_CORNER;
      }
    }

This is the trickiest part of the program. This is where the special cases associated with corners are handled. The first if statement checks to see if the wall to the right of the robot is still there. If the wall is gone, this is likely to be an outside corner. The robot has to be careful here to not run into the corner with the wheel. So if the wall is gone, then the next thing it checks is to see if the robot has cleared the corner by checking the MotorRotationCount of the right motor. If it is still too close to the corner then the robot continues straight with a steering value of 0. If it has cleared the corner then it is safe to turn so the steering value is set to 70, the value of STEER_OUTSIDE_CORNER.

If the wall is present, EOPD > 30, then we fall into the else in the middle of this chuck of code. The first thing in the else clause is to save the current MotorRotationCount to make note of where the wall is last seen. The next thing here is an if statement making sure the robot is not being forced to turn too sharply based on the EOPD value. I found that if the robot turns too sharply, the back of the robot tended to hit the back wall as it turned. The solution was to limit the amount of the turn.

The else clause in the bottom deals with the Ultrasonic sensor. So if the EOPD sensor is sort of within a normal tracking range, the wall is present but it is not too close, then the program checks the Ultrasonic sensor to see if the robot is approaching an inside corner. If there is a corner, then the steering value is forced to a value of -100. Actually it does do one other thing here. If the EOPD indicates that the robot is very close to the wall, then it can still set a steering value beyond -100. This can happen if in the middle of the turn the EOPD sensor gets very close to the turn, then the robot will force a sharper turn to avoid hitting the wall.

    TextOut(0, LCD_LINE5, "iSteer:     ");
    NumOut(6*7, LCD_LINE5, iSteer);

    pwrLeft = LimitPower(100+iSteer);
    pwrRight = LimitPower(100-iSteer);

    TextOut(0, LCD_LINE8, "                "); // Clear previous text
    NumOut(0,    LCD_LINE8, pwrRight);
    NumOut(6*12, LCD_LINE8, pwrLeft);

    OnFwd(LEFT_MOTOR, pwrLeft);
    OnFwd(RIGHT_MOTOR, pwrRight);
  }
}

Finally, the end of the loop and the end of the program. This is where the iSteer value gets used to set the power for the left and the right motors. The power for the left motor is simply 100+iSteer, and the right motor is 100-iSteer. Of course, this can result in power values that are outside the range -100 to 100. This is where the helper function LimitPower is used to make sure the power value is within the valid range.
Then finally the power is set to the motors.

Tags: , , , ,

3 Responses to “WallBot – NXC Version”

  1. [...] EOPD wall follower. Someone added one of the Mindsensors flyers to the end of the paper wall.  The wall follower [...]

  2. [...] zu unserem Roboter: Wir haben, glaube ich zumindest, schon einmal davon berichtet, dass wir einen Wallfollower Algorithmus für den NXT benutzen wollen und diesen in NXC umschreiben. Umgeschrieben haben wir ihn soweit und auch schon [...]

  3. [...] haben, glaube ich zumindest, schon einmal davon berichtet, dass wir einen Wallfollower Algorithmus für den NXT benutzen wollen und diesen in C umschreiben. Umgeschrieben haben wir ihn soweit und erste Erfolge [...]

Leave a Reply