/* File: objectFinder.ch the NXT robot will manange to track down objects within a spesific area with wandering off it */ #include #include #define FALSE 0 #define TRUE 1 /* flags for situations the robot encounters */ #define ERROR -1 #define NOTHING 0 #define OBJECTFOUND 1 #define DANGER 2 #define SAFE 3 CMindstorms robot; double speed = 3; // value set as the desired moving speed double radius = 2.2; // the radius of wheel double trackwidth = 13.6; // the distance between the two wheels double theta; // angle that motors turn to achieve the desired angle we want the robot to turn double angle, // difference between angle1 and angle2 angle1, // store the current angle of the left motor angle2; // store the last checked angle of the left motor double ultraValueLeft, // value of the left ultrasonic sensor reading ultraValueRight; // value of the right ultrasonic sensor reading int i; int status; // situation the robot in int lightValueLeft, // value of the left light sensor reading lightValueRight; // value of the right light sensor reading int moveToObject(); // self-defined function which guides the robot toward objects int moveAway(); // self-defined function which guides the robot to move away edges int moveArround(); // self-defined function which makes the robot to wander arround instead of going straing forward int checkStatus(); // self-defined function which is resposible of check the situation that the robot is in robot.setSensorLight(PORT1, "Reflect"); robot.setSensorLight(PORT4, "Reflect"); robot.setSensorUltrasonic(PORT2, "Centimeter"); robot.setSensorUltrasonic(PORT3, "Centimeter"); /* a for-loop filtering out values read within the first five times because usually these readings are incorrect */ for (i = 0; i < 5; i++) { robot.getSensorLight(PORT1, lightValueLeft); robot.getSensorLight(PORT4, lightValueRight); robot.getSensorUltrasonic(PORT2, ultraValueLeft); robot.getSensorUltrasonic(PORT3, ultraValueLeft); } theta = 360*trackwidth/2/radius; // convert the desired angle to the actual angle that two motors have to turn status = NOTHING; /* an infinite while-loop which keeps the robot running */ while (status != ERROR) { /* if-statements that determine which action to take based on the status the robot in */ if (status == NOTHING) { robot.setSpeed(speed, radius); robot.turnLeftNB(400, radius, trackwidth); robot.getJointAngleInstant(JOINT2, angle1); /* an while-loop that keeps the robot checking if it has reached the desired angle and not found anything yet */ while (status == NOTHING) { robot.getJointAngleInstant(JOINT2, angle2); angle = abs(angle2 - angle1); /* compare the angle has turned to the desired angle */ if (angle > theta) { status = SAFE; break; } status = checkStatus(); //printf("Looking for objects: status: %d\n", status); } robot.holdJoints(); } else if (status == SAFE) { status = moveArround(); } else if (status == DANGER) { status = moveAway(); } else if (status == OBJECTFOUND) { status = moveToObject(); } } int moveToObject() { double turnLeft, // correction value of left motor power turnRight; // correction value of right motor power double powerFactor, // the ration we want to multiply to the difference between the value of ultraValueLeft and ultraValueRight powerFactorMax = 0.4, // the adjustable desired maximum powerFactor powerFactorMin = 0; // the minimum powerFactor double ultraValueMax = 255, // the maximum possible ultrasonic sensor reading ultraValueMin = 0; // the minimum possible ultrasonic sensor reading double targetPower = 15; // the power level of both motors when the error is 0 and the robot goes straight forward int powerLeft, powerRight; /* a while-loop which keeps the robot to move toward an obstacle */ while (status == OBJECTFOUND) { robot.getSensorUltrasonic(PORT2, ultraValueLeft); robot.getSensorUltrasonic(PORT3, ultraValueRight); powerFactor = (powerFactorMax-powerFactorMin)*((ultraValueMax-ultraValueMin) - abs(ultraValueLeft-ultraValueRight))/(ultraValueMax-ultraValueMin); turnLeft = (ultraValueLeft - ultraValueRight)*powerFactor; turnRight = (ultraValueRight - ultraValueLeft)*powerFactor; powerLeft = round(turnLeft + targetPower); powerRight = round(turnRight + targetPower); robot.setJointPower(JOINT2, powerLeft); robot.setJointPower(JOINT3, powerRight); robot.moveForeverNB(); status = checkStatus(); //printf("Moving to the object: status: %d\n", status); } robot.holdJoints(); return status; } int moveAway() { double turnLeft, // correction value of left motor power turnRight; // correction value of right motor power double targetPower = 15; // the power level of both motors when the error is 0 and the robot goes straight forward double powerFactor = 1.4; // an arbitrary constant for calibrating output power int powerLeft, powerRight; turnLeft = (lightValueLeft - lightValueRight)*powerFactor; turnRight = (lightValueRight - lightValueLeft)*powerFactor; powerLeft = round(targetPower + turnLeft); powerRight = round(targetPower + turnRight); robot.setJointPower(JOINT2, -powerLeft); robot.setJointPower(JOINT3, -powerRight); robot.moveForeverNB(); robot.delaySeconds(1); robot.holdJoints(); status = SAFE; // after getting away from edges, set the status to SAFE return status; } int moveArround() { double timePassed = 0; // the time between the last time we checked the sensor and the time of the most recent check of the sensor double time1, // the system time we have recorded at the most recent time time2; // the last system time we have recorded double powerFactor = 0.1; // an arbitrary constant for calibrating output power double turnLeft, // correction value of left motor power turnRight; // correction value of right motor power double targetPower = 15; // the power level of both motors when the error is 0 and the robot goes straight forward int powerLeft, powerRight; int firstTime; // determine whether it is the first time a while-loop has been run firstTime = TRUE; robot.systemTime(time1); /* a while-loop which keeps the robot wandering for 6 seconds */ while (timePassed < 6) { robot.getSensorLight(PORT1, lightValueLeft); robot.getSensorLight(PORT4, lightValueRight); robot.systemTime(time2); /* an if-statement to tell if it is the first time that the while-loop has been executed */ if (firstTime == FALSE) { timePassed = time2 - time1; } turnLeft = (lightValueRight - lightValueLeft)*powerFactor; turnRight = (lightValueLeft - lightValueRight)*powerFactor; powerLeft = round(targetPower + turnLeft); powerRight = round(targetPower + turnRight); robot.setJointPower(JOINT2, powerLeft); robot.setJointPower(JOINT3, powerRight); robot.moveForeverNB(); status = checkStatus(); printf("Moving around: status: %d\n", status); /* if situation has changed from SAFE while the robot is moving arround */ if (status == DANGER || status == OBJECTFOUND) { robot.holdJoints(); return status; } firstTime = FALSE; } robot.holdJoints(); status = NOTHING; // nothing has been found during the wandering return status; } int checkStatus() { robot.getSensorLight(PORT1, lightValueLeft); robot.getSensorLight(PORT4, lightValueRight); robot.getSensorUltrasonic(PORT2, ultraValueLeft); robot.getSensorUltrasonic(PORT3, ultraValueLeft); if (lightValueLeft > 58 && lightValueRight > 58) { // the first priority is to keep the robot from wandering off a spesific area if (ultraValueLeft <= 100 || ultraValueRight <= 100) { // if either one of the ultrasonic sensor has found any object printf("left distance: %lf\n", ultraValueLeft); printf("right distance: %lf\n", ultraValueRight); return OBJECTFOUND; } else { // else, nothing has been found printf("nothing!\n"); return NOTHING; } } else { // the robot is at the edge of the area printf("danger!\n"); return DANGER; } return ERROR; // if sensors are reading wrong values }