1、The Sensor Project,Motor “Direction” together with sonar,Left motor,Table of Contents,I. Introduction 2 II. Positioning System 2 III. Sensory System 3 IV. Project Goal 4 V. Selecting The System 4 VI. Program The Brick 5 VII. Building The Robot 6 VIII. Project Code 7 IX. Performance Result 13 X. Futu
2、re Improvements 14 XI. Conclusion 15 XII. Reference 15 XIII. Appendix 16,Our first task mapping and navigation,The general problem of mobile robot navigation has to answer three questions: “Where am I?.” “Where am I going?,” “How should I get there?.” Our project will focus on how to achieve these t
3、hree main questions using a mobile robot system and at the same time map its surroundings. In order to answer the first and most fundamental question: “Where am I?,” the system requires some sort of build-in sensing equipments to position itself.,Positioning System,Most common positioning system in
4、robotic divides into two categories: relative and absolute position measurements and the brief description shown below:The Relative Position Measurements Uses internal sensor to measure its distant traveled relative to the previous position. The advantage is that its totally self-contained and does
5、not need any external system support to locate its position. On the other hand, its position error grows without bound unless and independent reference is used periodically to reduce the error. Two types of sensory system shown below are typically used for relative position measurement.,relative pos
6、ition measurement.,Two types of sensory system shown below are typically used for relative position measurement.Odometry This method uses encoders to measure wheel rotation and/or steering orientation. Inertial Navigation This method uses gyroscopes and accelerometers to measure rate of rotation and
7、 acceleration.,Absolute Position Measurements,The Absolute Position Measurements is using a “known” external environment to locate its absolute position in the map given like the global positioning system (GPS). The advantage is that it can navigate through the environment accurately. The disadvanta
8、ge is that:the location of source signal must be known or a detailed map of the environment must be given. There are few different types of system is using the absolute position measurement which are shown below,Active Beacons The robot computes its position from measuring the direction of incidence
9、 of three or more actively transmitted beacons. The transmitting signal could be light or radio frequencies.Artificial / Natural Landmark Recognition The robot identifies the landmark (artificial or natural) with a recognizable feature This feature is preset in the system to position itself in the k
10、nown map.Model Matching it is a complex matching system which identifies its surroundings with its internal map and world model of the environment to estimate its position. This is a more high level system It can: improve global maps based on the new sensory observations in a dynamic environment Int
11、egrate local maps into the global map to cover previously unexplored areas.,Absolute Position Measurements,Project Goal,Build a mobile robot using some of the sensory and compute method described previously to perform desired tasks. In this project we focus on two main goals:obstacle avoidance envir
12、onment mapping. The robot needs to be able to:navigate at an unknown environment, stop when there is an obstacle in the desire path, avoid the obstacle by changing route, map the environment without outside influence.,The NXT programmable microcomputer, so called the Brick. It has an Atmel 32-bit AR
13、M7 processor with 256KB Flash memory, 64KB RAM, and a speed of 48 MHz. The Brick also come four sensor inputs, three motor outputs connection, USB connection, LCD display with four bottoms controller, a loudspeaker, and Bluetooth capability.,Limited system capability,The Servo Motor it uses a built-
14、in incremental optical encoder to precisely control the motors output. The NXT allows to specify the rotating degree with an accurate of one degree, the rotating revolution, or run time at different output powers. The user can also synchronize between two motors and control using other internal read
15、ings.,Our use of motors,The Ultrasonic Sensor as described previously allows the robot to measure distances to its surroundings and avoid obstacles. The NXT ultrasonic sensor has a range of 255 cm and accuracy within 3 cm.,Our use of sensors,Programming in NXC,The most common C like compiler for NXT
16、 are RobotC and NXC (Not eXactly C). Since the RobotC requires license to use, the project robot was coded in NXC, which is an open source compilerThe NXT has a bytecode interpreter, which can be used to execute programs. The NXC compiler translates a source program into NXT bytecodes, which can the
17、n be executed on the target itself. Although the preprocessor and control structures of NXC are very similar to C, NXC is not a general-purpose programming language, and there are many restrictions that stem from limitations of the NXT bytecode interpreter. The lexical rules of the compiler and the
18、NXT input/output reading commands can be found at the “NXC Guide”.,NXC compiler interface,Project Code,The project code was written based on the concept of answering the three fundamental questions Where am I, Where am I going, and How should I get there and meeting the project goal at the same time
19、. The answer for those questions would be : “The robot is sitting on a x,y coordinates which start with 0,0 and always moving forward with tracking of orientation and distance on the best path which was decided after obstacles detection.”,Software,Calibration function “Distant_Check”,Calibration fun
20、ction “Distant_Check” * The program checks the speed of the robot at given surface and battery life. *The program starts with defining the ultrasonic sensor, input port 4, and driving motor as output port A -,This section set ultrasonic sensor in input port 4 and with a little time delay (0.05 secon
21、ds). Initiate counter i to 0.,- SetSensorType(US_IN, US); / US sensor in input port 4 Wait(50); i = 0; - This section show the ultrasonic sensor will take distance reading 1 second apart for 5 seconds and average the distance which is stored in dist_avg. - while (i 5)dist1 = SensorUS(US_IN);OnFwdSyn
22、c(Motor,50,0);Wait(1000);dist2 = SensorUS(US_IN);if(i = 0)dist_avg = (dist1-dist2);else dist_avg = (dist_avg+(dist1-dist2)/2;i+; -,Two measurements to make average,This last section of task maim outputs dist_avg on the NXT LCD screen until the user cancels the operation,- while (true) Off(Motor);Num
23、Out(0, LCD_LINE2, dist_avg); -,End of task main is here,“Rotation_Check”,Calibration function “Rotation_Check” * This program checks the rotation angle on the given surface and between left and right motor. It is used only for calibration *,The section of task main defines the left and right motor a
24、nd integer i.,- task main() #define R_Motor OUT_A #define L_Motor OUT_C int i; -,rotate the robot left for 4 times,- The following section will rotate the robot left 4 times with 45 degree each and next do the same rotating the robot to the right. - i = 0; while(i 4) RotateMotor(R_Motor,50,135);Off(
25、R_Motor);Wait(800);i+; i = 0; while(i 4) RotateMotor(L_Motor,50,135);Off(L_Motor);Wait(800);i+; -,End of task main,Main function “Mapping_Bot”,This is the main program for the project It creates the output data file It calls all the subroutines with user defined repeated number of times. The robot w
26、ill run for 5 seconds, and at the same time perform obstacle avoidance task and robot orientation for each second. After 5 seconds, the robot will sweep the surrounding from -90 to 90 degrees. Finally calculate its position and output robot position and sweeping result into the text file “Result.,Th
27、e program includes “My Position.nxc” which has all the subroutine codes. Defines function for ultrasonic sensor #include “My Position.nxc“;task main() string header;SetSensorType(US_IN, US); / US sensor in input port 4Wait(50);,MAIN,obstacle(),scan_record(),position(),if (CreateFile(“Result.txt“, 20
28、48, handle) = NO_ERR)TextOut(0,LCD_LINE2,“Mapping“);x = 0;y = 0;orit_d = 90; /preset to 90 degree, assume robot faces the +Y directionheader = “X, Ycm -90, -60, -30, 0, 30, 60, 90degree“;WriteLnString(handle, header, bytesWritten);repeat (7)obstacle();scan_record();position();CloseFile(handle);elsew
29、hile(true)TextOut(0,LCD_LINE2,“Error“); / end of task main,Creating new file name “Result.txt” and output “Mapping“ if success else output “Error” on the NXT LCD. The orientation of the robot is set to 90 degree which is the +Y direction. Function in the middle calls all the subroutines with user de
30、fined repeated times. The file is closed after all the subroutine is done, and it is ready to be exported out through USB or Bluetooth Obstacle avoidance Scan record Position calculation,Obstacle Avoid,Subroutine function “Obstacle Avoid” * The subroutine checks for obstacle within 30cm in the front
31、 of the ultrasonic sensor. The robot continues straight if no obstacle found, else finds the best path by checking distance to 40 degree to the left and right. Pick direction with longer path and rotate the robot 45 degree to that direction then continue straight if no obstacle found. *The program s
32、tart with define motor and sensor name and ports. Different integers are setup for the program to use, see common for each in detail. - #define Motor OUT_AC /driving motor #define R_Motor OUT_A /Right driving motor #define L_Motor OUT_C /Left driving motor #define US SENSOR_TYPE_LOWSPEED_9V /define
33、US as the Ultra Sonic sensor #define US_IN IN_4 /US sensor input #define Direction OUT_B /US sensor and driving direction motor #define rotate_angle 45 /angle rotated per termint opst; /distant to obstacle int left; / 40 degree to the left int right; / 40 degree to the right int RunTime; / total run
34、 time int run; /straight run time int orit5; /vector to record the orientation, size=5 int i_orit; / count for orientation int orit_d; /orientation in degree -,sub obstacle() RunTime = 0;i_orit = 0;while(i_orit 30) /check if obstacle within 30cmOnFwdSync(Motor,50,0); /continue FW if no obstacleWait(
35、run); /run 1 secRunTime += run; /calculate total run timeoriti_orit = orit_d; /set orit in its orientationi_orit+;,Start the subroutine, set initial value for i_orit to 0 (this is the main counter for the subroutine)i_oritis incremented for every 1 second the motor run forwards. The subroutine will
36、run for 5 seconds and record the orientation it run in for each second then stop. If no obstacle within 30 cm, the robot will continue to run forward until i_orit count reach 5.,elseOff(Motor); /stop motor if obstacle found/check best pathRotateMotor(Direction,50,-40);left = SensorUS(US_IN);RotateMo
37、tor(Direction,50,80);right = SensorUS(US_IN);if(left right) /pick best path (left or right)RotateMotor(L_Motor,50,135); /rotate 45 degree to the rightRotateMotor(Direction,50,-40);orit_d -= rotate_angle; /decrease orientation by 45 degreeelseRotateMotor(Direction,50,-80);RotateMotor(R_Motor,50,135);
38、 /rotate 45 degree to the leftRotateMotor(Direction,50,40);orit_d += rotate_angle; /increase orientation by 45 degreeOff(Motor); / end of sub obstacle,If there is obstacle within 30 cm, the robot will stop and check distance +/- 40 degrees from center and store the distant in int left & right. left
39、and right is compared for the best path (larger distane) and the robot will turn 45 degree to that direction with the help of front steering wheel trying towards the best path and then +/- the orientation angle by 45 degrees.,SUBROUTINE Dist Track,The subroutine perform distant scan from -90 to 90 d
40、egrees with 30 degree interval after every 5 second of the robot movements. The result of the scan is store in the string “dist_record”. *First section defines all the sensor and motor same as other program and some variable will be used in this program. - #define US SENSOR_TYPE_LOWSPEED_9V /define
41、US as the Ultra Sonic sensor #define Direction OUT_B /US sensor and driving direction motor #define Motor OUT_AC /driving motor #define US_IN IN_4 /US sensor input #define R_Motor OUT_A /Right driving motor #define L_Motor OUT_C /Left driving motor int dist; / distance of the US sensor in cm int sca
42、n_ang; /set scan angle byte handle; short bytesWritten; string dist_result; /result of distance from -90 to 90 degree,Check if motors stoped,sub scan_record() int i, dist1, dist2;/start checking distant if both left and right motor are offif(IOMA(OutputIOPower(R_Motor) = 0 ,The subroutine will check
43、 if all motor is stop, if so, scan angle will start from -90 degrees. And while the scan angle is less than 91 degree the ultrasonic sensor will read the distant 3 time and take the average before increment the scan angle by 30 degrees.,/store value string “dist_result“string dist_s = NumToStr(dist)
44、;if(scan_ang = -90)RotateMotor(Direction,50,30); /rotate direction to match scan angledist_result = StrCat(dist_s,“, “);if(scan_ang -90) RotateMotor(Direction,50,30); /rotate direction to match scan angledist_result = StrCat(dist_result,dist_s,“, “);if(scan_ang = 90) RotateMotor(Direction,50,-90); /rotate direction back to frontdist_result = StrCat(“,dist_result,dist_s,“ cm“);scan_ang += 30; /increment scan angle / end of subroutine scan_record,
