Hi community! I would like to introduce my new project “Room explorer with mapping functions”. The robot based upon a standard REM-Bot but in addition equipped with a hitechnic gyro, hitechnic compass sensor and an omni-wheel (look at the Pictures I will send when the server of RobotC will work again). First the robot moves about 360° to calibrate the compass using the gyro (thank you to Xander for code!). Then the robot moves its sonar-head to the right, to the left and in front position to get the distances according to its position. After doing this it turns around to the wall with the minimum distance and drives in front of it until sonar sensor detected a minimum sensor distance, e.g. 20cm. Then the robot turns parallel to the wall, moves his sonar-head to the right detecting the distance to the wall and drives counter clockwise parallel to the wall balancing distance. A mapping-task records the compass and odometry data every second and calculate the polar coordinates to cartesian coordinates (x,y). The coordinates are written as “map.txt”-file. So you can use Excel or an other programm to draw the path which the robot had moved. In addition to that you can follow the path at the NXT-LCD-screen. I had to choose a scale for it, so you have to suit the scale to your room size. If the robots touch sensor detected an obstacle the robot moves back and turn left for 90 degrees and continuous his explorer-duty allways running counter clockwise with wall to the right. How to expect the end of path doesn’t suit exactly to the beginning because of inaccuracies of compass and odometry measures. Have fun to test my programm and let me know if you have some hints for me. Do anybody have experience with a hitechnic accelerator-sensor to measure/calculate the distance by integrating? Is this method an alternative and more accurate as odometry data?
Merry chrsitmas!
Sigtrygg
 |  |  |  | Code: #pragma config(Sensor, S1, touchSensor, sensorTouch) #pragma config(Sensor, S2, HTCOMPASS, sensorI2CCustom) #pragma config(Sensor, S3, HTGYRO, sensorAnalogInactive) #pragma config(Sensor, S4, sonarSensor, sensorSONAR) #pragma config(Motor, motorA, sonarMotor, tmotorNXT, PIDControl, encoder) #pragma config(Motor, motorB, rightMotor, tmotorNXT, PIDControl, encoder) #pragma config(Motor, motorC, leftMotor, tmotorNXT, PIDControl, encoder) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//Roomexplorer and mapping robot by Sigtrygg, December 2013
/* Credits : * Special thanks to to Xander Soldaat for gyro and compass-drivers and gyro/compass example codes! * * License: You may use this code as you wish. */
#include "REM-Bot_Roomexplorer_functions_1.3.h" //all necessary functions are written in the header-file
//*************************************************************************************************
task mapping() { eraseDisplay(); time10[T2]=0; maxtime=(long)(nFileSize)*interval/(60*17);//in minutes //Prepare file-handling hFileHandle = 0; Delete(sFileName, nIoResult); OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);
while (mode_exit !=1) { odometryValueOld=nMotorEncoder[rightMotor]; //eraseDisplay(); valTime10 = time10[T2]; recordtime=(recordtime+valTime10/100)/60; //nxtDisplayStringAt(0, 60,"Recordtime: %d",recordtime); //nxtDisplayStringAt(0, 50,"Max.time: %d",maxtime); //nxtDisplayStringAt(0, 40,"Sonar: %d",SensorValue(sonarSensor)); //nxtDisplayStringAt(0, 30,"Heading: %d",compassValue); wait10Msec(interval*100);//write data according interval! compassValue=HTMCreadHeading(HTCOMPASS);//get current heading
if (recordtime==maxtime || nNxtButtonPressed==3)//if recordtime is reached or the orange Key is pressed stop! { wait1Msec(500); Close(hFileHandle, nIoResult);
mode_exit=1;//when reaching the max. recordtime, exit with closing file } if (touchflag==0)//only write coordinates if touch sensor is 0 and after robot has repositioned { odometryValueCurrent=nMotorEncoder[rightMotor]; odometryValueRadius=(odometryValueCurrent-odometryValueOld)*WHEELCIRCUMFERENCE/360;//radius in cm //calculate cartesian coordinates according to start point cartesX+=-(cosDegrees(compassValue)*odometryValueRadius);//calculating with float!! cartesY+=(sinDegrees(compassValue)*odometryValueRadius); //convert int values to string values for file-writing StringFormat(scartesX, "%d", round(cartesX));//show only rounded integer StringFormat(scartesY, "%d", round (cartesY)); //convert int values to string values for file-writing (Polar) StringFormat(sodometryValueRadius, "%d", round(odometryValueRadius)); StringFormat(scompassValue, "%d", round(compassValue)); //write cartesian coordinates to file WriteText(hFileHandle, nIoResult, "\r\n"); WriteText(hFileHandle, nIoResult, sodometryValueRadius); WriteText(hFileHandle, nIoResult, ","); WriteText(hFileHandle, nIoResult, scompassValue); WriteText(hFileHandle, nIoResult, ","); WriteText(hFileHandle, nIoResult, scartesX); WriteText(hFileHandle, nIoResult, ","); WriteText(hFileHandle, nIoResult, scartesY); //Draw Map-Pixels on screen Pixel_x=round((cartesX+PixelOffset_x)*DRAW_SCALE); Pixel_y=round((cartesY+PixelOffset_y)*DRAW_SCALE); nxtSetPixel(Pixel_x, Pixel_y); } } sonarLeftPosition(); wait1Msec(500); Close(hFileHandle, nIoResult);
while(nNxtButtonPressed!=0)//wait until escape button is pressed to take a look at the map { motor[rightMotor]=0; motor[leftMotor]=0; } } //*****************************************************************************************************************
task main() { //Calibrate the gyro, make sure you hold the sensor still wait1Msec(2000); HTGYROstartCal(HTGYRO); //returns offset of Gyro (e.g. 610), it depends on temperature and other environmental influences. wait1Msec(1000);
//Calibrate the compass compass_calibration();
//----------------------------- behavior ---------------------------------------------------------
//check in which direction the nearest wall is and drive to it. Then turn 90° left for (int i=0; i<3; i++) { sonarangle[0]=0; sonarturnvelocity[0]=0; sonarangle[1]=92; sonarturnvelocity[1]=15; sonarangle[2]=-180; sonarturnvelocity[2]=-15; nMotorEncoder[sonarMotor] = 0; nMotorEncoderTarget[sonarMotor] = sonarangle[i]; motor[sonarMotor] = sonarturnvelocity[i]; while (nMotorRunState[sonarMotor] != runStateIdle) { // do not continue } motor[sonarMotor] = 0; //get the average of 10 measures for (int t=0;t<10;t++) { wait1Msec(50); distancevalue[i]=SensorValue(sonarSensor); distancevaluesum+=distancevalue[i]; }
distancevalue[i]=distancevaluesum/10; nxtDisplayCenteredBigTextLine(4, "%2d",distancevalue[i]); wait1Msec(1500); distancevaluesum=0; eraseDisplay(); // measure end } //get minimum distance and its angle minimumdistance=256; for (int i=0; i<3; i++) { if (distancevalue[i]<minimumdistance) { minimumdistance=distancevalue[i]; firstgodirection=sonarangle[i]; } } //get sonar motor back to 0 degree position nMotorEncoder[sonarMotor] = 0; nMotorEncoderTarget[sonarMotor] =92; motor[sonarMotor] =15; while (nMotorRunState[sonarMotor] != runStateIdle) { // do not continue } motor[sonarMotor] = 0; eraseDisplay(); nxtDisplayCenteredBigTextLine(2, "%3d",firstgodirection); wait1Msec(2000);
//turn to direction of minimum distance switch(firstgodirection) { case 92: turn(90,'r',TURN_VELOCITY); break; case -180: turn(90, 'l',TURN_VELOCITY); break; }
//start mapping StartTask(mapping);
//drive ahead to the wall until sonar distance=20cm wait1Msec(500); forward_until_sonardetection(20); wait1Msec(500); turn(90,'l',TURN_VELOCITY);
//get sonar back to front position (90 degrees) sonarRightPosition();
/*drive along the wall and hold distance to it for 25cm and stop, if touchSensor detected bumping, then drive back, turn 90° and continue driving beneath the wall until the orange button is pressed or record-time is reached*/
driveBySonarSideCheck(); }
|  |  |  |  |
HEADER-FILE:
 |  |  |  | Code: //REM-Bot with hitechnic gyro, hitechnic compass and touch sensor //Roomexplorer and mapping robot by Sigtrygg, December 2013
/* Credits : * Special thanks to to Xander Soldaat for gyro and compass-drivers and gyro/compass example codes! * * License: You may use this code as you wish. */
#include "drivers/hitechnic-gyro.h" #include "drivers/hitechnic-compass.h"
//Constants REM-Bot const float WHEELCIRCUMFERENCE = 17.7; //(cm) const float WHEELTRACK = 12.06; //Trackwidth (cm) const int TURN_VELOCITY = 20; const int EXPLORE_VELOCITY=15;
//globals float Geschw_verh=0; //int Geschw=0; //int Drehradius=0; int sonarangle[3];//angle of sonarhead (0, 90, 180) int distancevalue[3];//array for the distances in three directions (0, 90, 180) int sonarturnvelocity[3];//speed of sonarhead-motor int distancevaluesum=0;//sum of measured distances int minimumdistance=0;//nearest distance to next wall int firstgodirection=0;//direction to the nearest wall int minimumSonarDistance=28; //minimum distance to wall = 28cm int touchflag=0;//if touch sensor is actiive don't mapping
//variable for map drawing on screen (according to livingroom) int Pixel_x=0; int Pixel_y=0; const float DRAW_SCALE=0.15;//0.1767; const float PixelOffset_x=345.0; const float PixelOffset_y=190.0;
int interval=1; //continous mode writes data every 1 seconds int maxtime=0; int recordtime=0; long valTime10=0; int mode_exit=0;
// define values for polar coordinates int compassValue=0; //angle of current heading int odometryValueOld=0; //for determine radius for polar coordinates int odometryValueCurrent=0; //for determine radius for polar coordinates float odometryValueRadius=0; //has to be float, because of needing precise values for calculating cartesian coordinates
// define values for cartesian coordinates float cartesX=0; float cartesY=0; string scartesX = ""; string scartesY = "";
string sodometryValueRadius=""; string scompassValue="";
// File handling TFileIOResult nIoResult; TFileHandle hFileHandle; // NOTE ON FILESIZE: Every data point added is about 17 bytes in size. int nFileSize = 10000; //about 10 min datalogging with an interval of 1s const string sFileName = "map.txt";
//=========== DECLARE FUNCTIONS ================================================ //******* FUNCTION drive ahead ************************************************* void driveAhead(int velocity, float distance);
//******* FUNCTION drive back ************************************************** void driveBack(int velocity, float distance);
//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor *** void turn(int degrees, char direction , int velocity);
//******* FUNCTION turn left ************************************************** void turnLeft(int Turnvelocity, int Turnangle);
//******* FUNCTION turn right ************************************************* void turnRight(int Turnvelocity, int Turnangle);
//******* FUNCTION forward_until_touch ***************************************** void forward_until_touch();
//******* FUNCTION forward_until_sonar_detection ******************************* void forward_until_sonardetection(int sonar_distance);
//******* FUNCTION turn right with radius ************************************** void rechtsDrehung(int Geschw,int Drehradius);
//******* FUNCTION turn left with radius *************************************** void linksDrehung(int Geschw, int Drehradius);
//******* FUNCTION turn left with radius for a specific angle ****************** void linksDrehungWinkel(int Geschw, int Drehradius, int angle);
//******* FUNCTION COMPASS CALIBRATION ***************************************** void compass_calibration();
//******* FUNCTION Drive by sonar side check *********************************** void driveBySonarSideCheck();
//******* FUNCTION Sonar-head to right position ******************************** void sonarRightPosition();
//******* FUNCTION Sonar-head to left position ******************************** void sonarLeftPosition();
//==============================================================================
//******* FUNCTION drive ahead ************************************** //void driveAhead(int velocity, float distance) //{ // nSyncedMotors = synchBC; // sync motors B and C so that B is the master and C is the slave // nSyncedTurnRatio = 100;
// float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE; // nMotorEncoder[rightMotor] = 0; // nMotorEncoderTarget[rightMotor] = Angle_sum; // motor[rightMotor] = velocity; // while (nMotorRunState[rightMotor] != runStateIdle) // { // // do not continue // } // motor[rightMotor] = 0; //}
//******* FUNCTION drive ahead ************************************** //void driveAhead(int velocity, float distance) //{ // nSyncedMotors = synchBC; // sync motors B and C so that B is the master and C is the slave // nSyncedTurnRatio = 100;
// float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE; // nMotorEncoder[rightMotor] = 0;
// while (nMotorEncoder[rightMotor] < Angle_sum) // { // motor[rightMotor] = velocity; // } // motor[rightMotor] = 0; //}
//******* FUNCTION drive ahead ************************************** void driveAhead(int velocity, float distance) {
float Angle_sum = distance * 360 / WHEELCIRCUMFERENCE; nMotorEncoder[rightMotor] = 0; nMotorEncoder[leftMotor] = 0; nMotorEncoderTarget[rightMotor] = Angle_sum; nMotorEncoderTarget[leftMotor] = Angle_sum; motor[rightMotor] = velocity; motor[leftMotor] = velocity; while (nMotorRunState[rightMotor] != runStateIdle || nMotorRunState[leftMotor] != runStateIdle ) { // do not continue } motor[rightMotor] = 0; motor[leftMotor] = 0; }
//******* FUNCTION drive back ************************************** void driveBack(int velocity, float distance) { float Angle_sum = -distance * 360 / WHEELCIRCUMFERENCE; nMotorEncoder[rightMotor] = 0; nMotorEncoder[leftMotor] = 0; nMotorEncoderTarget[rightMotor] = Angle_sum; nMotorEncoderTarget[leftMotor] = Angle_sum; motor[rightMotor] = -velocity; motor[leftMotor] = -velocity; while (nMotorRunState[rightMotor] != runStateIdle || nMotorRunState[leftMotor] != runStateIdle ) { // do not continue } motor[rightMotor] = 0; motor[leftMotor] = 0; }
//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor *** void turn(int degrees, char direction, int velocity) {
//correction is necessary because of drifting !! int correction=0;
if (degrees==90) correction=3; //three degrees correction
float rotSpeed = 0; float heading = 0; nSyncedMotors = synchNone;
// Reset the timer. ClearTimer(T1);
while (abs(heading) < (degrees-correction)) { // Wait until 20ms has passed while (time1[T1] < 20) wait1Msec(1);
// Reset the timer ClearTimer(T1);
// Read the current rotation speed rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees // we've turned in the last 20ms // If our current rate of rotation is 100 degrees/second, // then we will have turned 100 * (20/1000) = 2 degrees since // the last time we measured. heading += rotSpeed * 0.02;
// Display our current heading on the screen //nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);
if (direction == 'r') { motor[rightMotor] = -velocity; motor[leftMotor] = velocity; } else if (direction == 'l') { motor[rightMotor] = velocity; motor[leftMotor] = -velocity; } }
//stop motor[rightMotor] = 0; motor[leftMotor] = 0; }
//******* FUNCTION turn left ******************** void turnLeft(int Turnvelocity, int Turnangle) { nMotorEncoder[leftMotor] = 0; while (nMotorEncoder[leftMotor] < Turnangle) { motor[rightMotor] = Turnvelocity; motor[leftMotor] = -Turnvelocity;
} motor[rightMotor] = 0; motor[leftMotor] = 0; }
//******* FUNCTION turn right ******************** void turnRight(int Turnvelocity, int Turnangle) { nMotorEncoder[leftMotor] = 0; while (nMotorEncoder[leftMotor] < Turnangle) { motor[rightMotor] = -Turnvelocity; motor[leftMotor] = Turnvelocity;
} motor[rightMotor] = 0; motor[leftMotor] = 0; }
//******* FUNCTION forward_until_touch ************************************** void forward_until_touch() { nSyncedMotors = synchBC; nSyncedTurnRatio = 100; while (SensorValue(touchSensor) != 1) { motor[rightMotor] = 30; } motor[rightMotor] = 0; }
//******* FUNCTION forward_until_sonar_detection ***************************** void forward_until_sonardetection(int sonar_distance) { nSyncedMotors = synchBC; nSyncedTurnRatio = 100; while (SensorValue(sonarSensor) > sonar_distance) { motor[rightMotor] = EXPLORE_VELOCITY; } motor[rightMotor] = 0; }
//******* FUNCTION turn right with radius ************************************ void rechtsDrehung(int Geschw,int Drehradius) { Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2); motor[rightMotor] = Geschw/Geschw_verh; motor[leftMotor] = Geschw; }
//******* FUNCTION turn left with radius ************************************ void linksDrehung(int Geschw, int Drehradius) { Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2); motor[rightMotor] = Geschw; motor[leftMotor] = Geschw/Geschw_verh; }
//******* FUNCTION turn left with radius for a specific angle ************************************ void linksDrehungWinkel(int Geschw, int Drehradius, int angle) { Geschw_verh=(Drehradius+WHEELTRACK/2)/(Drehradius-WHEELTRACK/2); nMotorEncoder[rightMotor] = 0;
int Kreisumfang=2*3.141*(Drehradius); //den das rechte Rad beschreibt in cm int Turnangle=(Kreisumfang/WHEELCIRCUMFERENCE)*angle;
while (nMotorEncoder[rightMotor] < Turnangle) { motor[rightMotor] = Geschw; motor[leftMotor] = Geschw/Geschw_verh; } }
//******* FUNCTION COMPASS CALIBRATION ************************************ void compass_calibration() {
wait1Msec(2000);
if (!HTMCstartCal(HTCOMPASS)) { eraseDisplay(); nxtDisplayTextLine(1, "ERROR: Couldn't"); nxtDisplayTextLine(2, "calibrate sensor."); nxtDisplayTextLine(4, "Check connection"); nxtDisplayTextLine(5, "and try again."); PlaySound(soundException); while(bSoundActive) EndTimeSlice(); wait1Msec(5000); StopAllTasks(); }
turn(365, 'r', 5);//turn right a little bit over 360 degree at 5% speed using gyro
motor[rightMotor] = 0; motor[leftMotor] = 0;
if (!HTMCstopCal(HTCOMPASS)) { eraseDisplay(); nxtDisplayTextLine(1, "ERROR: Calibration"); nxtDisplayTextLine(2, "has failed."); nxtDisplayTextLine(4, "Check connection"); nxtDisplayTextLine(5, "and try again."); PlaySound(soundException); while(bSoundActive) EndTimeSlice(); wait1Msec(5000); StopAllTasks(); } else { nxtDisplayTextLine(1, "SUCCESS: "); nxtDisplayTextLine(2, "Calibr. done."); PlaySound(soundUpwardTones); while(bSoundActive) EndTimeSlice(); wait1Msec(5000); } eraseDisplay();
}
//******* FUNCTION Drive by sonar side check ************************************************
void driveBySonarSideCheck() { while (true) { while (SensorValue(touchSensor) < 1) { wait1Msec(50); if (SensorValue(sonarSensor) > minimumSonarDistance) { rechtsDrehung(EXPLORE_VELOCITY,30); } else if (SensorValue(sonarSensor) < minimumSonarDistance) { linksDrehung(EXPLORE_VELOCITY,30); } else { motor[rightMotor]=EXPLORE_VELOCITY; motor[leftMotor]=EXPLORE_VELOCITY; } } touchflag=1; motor[rightMotor]=0; motor[leftMotor]=0; driveBack(EXPLORE_VELOCITY, 10); wait1Msec(250); turn(90,'l',TURN_VELOCITY); touchflag=0; } mode_exit=1;//end mapping and close data-file } //******* FUNCTION Sonar-head to right position ******************* void sonarRightPosition() { //get sonar motor to 90 degree position nMotorEncoder[sonarMotor] = 0; nMotorEncoderTarget[sonarMotor] = 92; motor[sonarMotor] =25; while (nMotorRunState[sonarMotor] != runStateIdle) { // do not continue } motor[sonarMotor] = 0; wait1Msec(150); }
//******* FUNCTION Sonar-head to left position ******************* void sonarLeftPosition() { //get sonar motor to 90 degree position nMotorEncoder[sonarMotor] = 0; nMotorEncoderTarget[sonarMotor] = -92; motor[sonarMotor] =-25; while (nMotorRunState[sonarMotor] != runStateIdle) { // do not continue } motor[sonarMotor] = 0; wait1Msec(150); }
|  |  |  |  |
|