GP2D12 to a RCX Example

Related
Products

Product image for Sharp GP2D12 Detector Package

LEGO Wallfollower built from instructions.

Introduction

As a direct application to my GP2D12 distance sensor , I built this wall follower. Its base is the Constructopedia RoverBot. I only added a turret bearing the sensor. The trick here is to direct sensor beam 45° away to also detect walls in front of vehicle. A simple program that maintains constant distance between wall and sensor creates a wall-following behaviour.

Projected path of Wallfollowing during program execution.

Contents

Source Code

The program is written with NQC. It must be compiled and downloaded in RCX before using it. I use the RCX command center shell to edit the source files.

/* filename: GP2D12-3-c.nqc */ // Motor speed and direction #define MFWD 3 #define MFLT 2 #define MOFF 1 #define MREV 0 // Distance thresholds #define XL3 527 #define XL2 480 #define XL1 444 #define XR1 430 #define XR2 420 #define XR3 410 int value, x, dx; int motl, motr; task main() { SetSensor(SENSOR_1,SENSOR_LIGHT); Wait(100); // Charge GP2D12 sensor SetUserDisplay(value,0); // Display distance (remove for RCX firmware 1.5) SetSensor(SENSOR_1,SENSOR_TOUCH); // Activate sensor Wait(5); SetSensorMode(SENSOR_1,SENSOR_MODE_RAW); value=SENSOR_1; // Get distance SetSensor(SENSOR_1,SENSOR_LIGHT); // Re-cahrge sensor for next time Wait(25); x=value; dx=0; until (false) { SetSensor(SENSOR_1,SENSOR_TOUCH); Wait(5); SetSensorMode(SENSOR_1,SENSOR_MODE_RAW); value=SENSOR_1; // Get distance SetSensor(SENSOR_1,SENSOR_LIGHT); // and recharge Wait(15); // shorter delay because calculations take time! dx=x-value; //Distance variarion x=value; motl=MFWD; motr=MFWD; if (x>=XL1) // Close to the wall { if (x>=XL3) { motr=MREV; // Very close, turn in place } else if (x>=XL2) { motr=MOFF; // close enough, turn } else if (x>=XL1) { motr=MFLT; // a bit too close, shallow turn } } else // far from the wall { if (x<=XR2) // far from wall, { if (dx<2) // if small variation turn (avoids turning too short on salient angles) motl=MOFF; } else if (x<=XR1) { motl=MFLT; // a bit too far, shallow turn } } switch(motr) { case MFWD: OnFwd(OUT_C); break; case MFLT: Float(OUT_C); break; case MOFF: Off(OUT_C); break; case MREV: OnRev(OUT_C); break; } switch(motl) { case MFWD: OnFwd(OUT_A); break; case MFLT: Float(OUT_A); break; case MOFF: Off(OUT_A); break; case MREV: OnRev(OUT_A); break; } } }

Revision History:

2001-04-27: Example Created.