├── .gitattributes ├── .gitignore ├── LICENCE ├── README.md ├── assets ├── 1.jpg ├── 2.jpg └── 3.jpg ├── pioneer3dx bug0 ├── controllers │ ├── obstacle_avoidance_with_kinect │ │ ├── .obstacle_avoidance_with_kinect.exe.signature │ │ ├── Makefile │ │ ├── obstacle_avoidance_with_kinect.c │ │ ├── obstacle_avoidance_with_kinect.d │ │ ├── obstacle_avoidance_with_kinect.exe │ │ ├── obstacle_avoidance_with_kinect.exe_webots │ │ └── obstacle_avoidance_with_kinect.o │ └── supervisor │ │ ├── Makefile │ │ ├── supervisor.c │ │ ├── supervisor.d │ │ ├── supervisor.exe │ │ ├── supervisor.exe_webots │ │ └── supervisor.o ├── protos │ ├── Pioneer_DistanceSensor.proto │ └── textures │ │ └── honeycomb.png └── worlds │ ├── .pioneer3dx_with_kinect.wbproj │ ├── .pioneer3dx_with_kinect.wbt.project │ ├── pioneer3dx_with_kinect.wbt │ └── textures │ ├── body_3dx.png │ ├── caster_wheel.png │ ├── parquet.png │ ├── serial_plug.png │ ├── smooth_rubber.png │ └── top_3dx.png ├── pioneer3dx bug1 ├── controllers │ ├── obstacle_avoidance_with_kinect │ │ ├── .obstacle_avoidance_with_kinect.exe.signature │ │ ├── Makefile │ │ ├── obstacle_avoidance_with_kinect.c │ │ ├── obstacle_avoidance_with_kinect.d │ │ ├── obstacle_avoidance_with_kinect.exe │ │ ├── obstacle_avoidance_with_kinect.exe_webots │ │ └── obstacle_avoidance_with_kinect.o │ └── supervisor │ │ ├── Makefile │ │ ├── supervisor.c │ │ ├── supervisor.d │ │ ├── supervisor.exe │ │ ├── supervisor.exe_webots │ │ └── supervisor.o ├── protos │ ├── Pioneer_DistanceSensor.proto │ └── textures │ │ └── honeycomb.png └── worlds │ ├── .pioneer3dx_with_kinect.wbt.project │ ├── pioneer3dx_with_kinect.wbt │ └── textures │ ├── body_3dx.png │ ├── caster_wheel.png │ ├── parquet.png │ ├── serial_plug.png │ ├── smooth_rubber.png │ └── top_3dx.png └── pioneer3dx bug2 ├── controllers ├── obstacle_avoidance_with_kinect │ ├── .obstacle_avoidance_with_kinect.exe.signature │ ├── Makefile │ ├── obstacle_avoidance_with_kinect.c │ ├── obstacle_avoidance_with_kinect.d │ ├── obstacle_avoidance_with_kinect.exe │ ├── obstacle_avoidance_with_kinect.exe_webots │ └── obstacle_avoidance_with_kinect.o └── supervisor │ ├── Makefile │ ├── supervisor.c │ ├── supervisor.d │ ├── supervisor.exe │ ├── supervisor.exe_webots │ └── supervisor.o ├── protos ├── Pioneer_DistanceSensor.proto └── textures │ └── honeycomb.png └── worlds ├── .pioneer3dx_with_kinect.wbt.project ├── pioneer3dx_with_kinect.wbt └── textures ├── body_3dx.png ├── caster_wheel.png ├── parquet.png ├── serial_plug.png ├── smooth_rubber.png └── top_3dx.png /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ################# 2 | ## Eclipse 3 | ################# 4 | 5 | *.pydevproject 6 | .project 7 | .metadata 8 | bin/ 9 | tmp/ 10 | *.tmp 11 | *.bak 12 | *.swp 13 | *~.nib 14 | local.properties 15 | .classpath 16 | .settings/ 17 | .loadpath 18 | 19 | # External tool builders 20 | .externalToolBuilders/ 21 | 22 | # Locally stored "Eclipse launch configurations" 23 | *.launch 24 | 25 | # CDT-specific 26 | .cproject 27 | 28 | # PDT-specific 29 | .buildpath 30 | 31 | 32 | ################# 33 | ## Visual Studio 34 | ################# 35 | 36 | ## Ignore Visual Studio temporary files, build results, and 37 | ## files generated by popular Visual Studio add-ons. 38 | 39 | # User-specific files 40 | *.suo 41 | *.user 42 | *.sln.docstates 43 | 44 | # Build results 45 | 46 | [Dd]ebug/ 47 | [Rr]elease/ 48 | x64/ 49 | build/ 50 | [Bb]in/ 51 | [Oo]bj/ 52 | 53 | # MSTest test Results 54 | [Tt]est[Rr]esult*/ 55 | [Bb]uild[Ll]og.* 56 | 57 | *_i.c 58 | *_p.c 59 | *.ilk 60 | *.meta 61 | *.obj 62 | *.pch 63 | *.pdb 64 | *.pgc 65 | *.pgd 66 | *.rsp 67 | *.sbr 68 | *.tlb 69 | *.tli 70 | *.tlh 71 | *.tmp 72 | *.tmp_proj 73 | *.log 74 | *.vspscc 75 | *.vssscc 76 | .builds 77 | *.pidb 78 | *.log 79 | *.scc 80 | 81 | # Visual C++ cache files 82 | ipch/ 83 | *.aps 84 | *.ncb 85 | *.opensdf 86 | *.sdf 87 | *.cachefile 88 | 89 | # Visual Studio profiler 90 | *.psess 91 | *.vsp 92 | *.vspx 93 | 94 | # Guidance Automation Toolkit 95 | *.gpState 96 | 97 | # ReSharper is a .NET coding add-in 98 | _ReSharper*/ 99 | *.[Rr]e[Ss]harper 100 | 101 | # TeamCity is a build add-in 102 | _TeamCity* 103 | 104 | # DotCover is a Code Coverage Tool 105 | *.dotCover 106 | 107 | # NCrunch 108 | *.ncrunch* 109 | .*crunch*.local.xml 110 | 111 | # Installshield output folder 112 | [Ee]xpress/ 113 | 114 | # DocProject is a documentation generator add-in 115 | DocProject/buildhelp/ 116 | DocProject/Help/*.HxT 117 | DocProject/Help/*.HxC 118 | DocProject/Help/*.hhc 119 | DocProject/Help/*.hhk 120 | DocProject/Help/*.hhp 121 | DocProject/Help/Html2 122 | DocProject/Help/html 123 | 124 | # Click-Once directory 125 | publish/ 126 | 127 | # Publish Web Output 128 | *.Publish.xml 129 | *.pubxml 130 | 131 | # NuGet Packages Directory 132 | ## TODO: If you have NuGet Package Restore enabled, uncomment the next line 133 | #packages/ 134 | 135 | # Windows Azure Build Output 136 | csx 137 | *.build.csdef 138 | 139 | # Windows Store app package directory 140 | AppPackages/ 141 | 142 | # Others 143 | sql/ 144 | *.Cache 145 | ClientBin/ 146 | [Ss]tyle[Cc]op.* 147 | ~$* 148 | *~ 149 | *.dbmdl 150 | *.[Pp]ublish.xml 151 | *.pfx 152 | *.publishsettings 153 | 154 | # RIA/Silverlight projects 155 | Generated_Code/ 156 | 157 | # Backup & report files from converting an old project file to a newer 158 | # Visual Studio version. Backup files are not needed, because we have git ;-) 159 | _UpgradeReport_Files/ 160 | Backup*/ 161 | UpgradeLog*.XML 162 | UpgradeLog*.htm 163 | 164 | # SQL Server files 165 | App_Data/*.mdf 166 | App_Data/*.ldf 167 | 168 | ############# 169 | ## Windows detritus 170 | ############# 171 | 172 | # Windows image file caches 173 | Thumbs.db 174 | ehthumbs.db 175 | 176 | # Folder config file 177 | Desktop.ini 178 | 179 | # Recycle Bin used on file shares 180 | $RECYCLE.BIN/ 181 | 182 | # Mac crap 183 | .DS_Store 184 | 185 | 186 | ############# 187 | ## Python 188 | ############# 189 | 190 | *.py[co] 191 | 192 | # Packages 193 | *.egg 194 | *.egg-info 195 | dist/ 196 | build/ 197 | eggs/ 198 | parts/ 199 | var/ 200 | sdist/ 201 | develop-eggs/ 202 | .installed.cfg 203 | 204 | # Installer logs 205 | pip-log.txt 206 | 207 | # Unit test / coverage reports 208 | .coverage 209 | .tox 210 | 211 | #Translations 212 | *.mo 213 | 214 | #Mr Developer 215 | .mr.developer.cfg 216 | -------------------------------------------------------------------------------- /LICENCE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2012 Can Guney Aksakalli http://web.itu.edu.tr/aksakallic/ 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BugAlgorithms 2 | 3 | The implementation of Bug Motion planning algorithms in [Webots](https://www.cyberbotics.com/) robot simulation environment 4 | 5 | [[The article for the project](https://aksakalli.github.io/2012/05/16/bug-motion-planning-algorithms.html)] 6 | 7 | ![bug2 motion planning path](https://raw.githubusercontent.com/aksakalli/BugAlgorithms/master/assets/3.jpg) 8 | 9 | ## Videos 10 | 11 | See in action: 12 | 13 | * Bug0 https://youtu.be/C6GmD4qS3bs 14 | * Bug1 https://youtu.be/iJWULA_gIy8 15 | * Bug2 https://youtu.be/Z5-TBsKPCF0 16 | 17 | ## License 18 | 19 | Released under [the MIT license](LICENSE). 20 | -------------------------------------------------------------------------------- /assets/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/assets/1.jpg -------------------------------------------------------------------------------- /assets/2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/assets/2.jpg -------------------------------------------------------------------------------- /assets/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/assets/3.jpg -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/.obstacle_avoidance_with_kinect.exe.signature: -------------------------------------------------------------------------------- 1 | 15532e9c-c2e75a40-19fb921e-94b080e5-81eb4c0a 2 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c: -------------------------------------------------------------------------------- 1 | /* 2 | * File: obstacle_avoidance_with_kinect.c 3 | * Date: August 24th, 2011 4 | * Description: A Braitenberg-like controller moving a Pioneer 3-DX equipped with a kinect. 5 | * Author: can 6 | * 7 | * Copyright (c) 2011 Cyberbotics - www.cyberbotics.com 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define MAX_SPEED 10.0 21 | #define CRUISING_SPEED 7.5 22 | #define TOLERANCE -0.1 23 | #define OBSTACLE_THRESHOLD 0.5 24 | #define SLOWDOWN_FACTOR 0.5 25 | 26 | static WbDeviceTag front, center; 27 | static WbDeviceTag leftWheel, rightWheel; 28 | static WbDeviceTag kinect; 29 | const float* kinectValues; 30 | static int time_step = 64; 31 | const double *point1; 32 | const double *point2; 33 | static WbDeviceTag distance_sensor[8]; 34 | double distance_value[8]; 35 | int main() 36 | { 37 | wb_robot_init(); 38 | time_step = wb_robot_get_basic_time_step(); 39 | leftWheel = wb_robot_get_device("leftWheel"); 40 | rightWheel = wb_robot_get_device("rightWheel"); 41 | kinect = wb_robot_get_device("kinect"); 42 | wb_camera_enable(kinect, time_step); 43 | int kinectWidth = wb_camera_get_width(kinect); 44 | int kinectHeight = wb_camera_get_height(kinect); 45 | int halfWidth = kinectWidth/2; 46 | int viewHeight = kinectHeight/2 + 10; 47 | double maxRange = wb_camera_get_max_range(kinect); 48 | double rangeThreshold = 0.7; 49 | double invMaxRangeTimesWidth = 1.0 / (maxRange * kinectWidth); 50 | static float targetX=0; 51 | static float targetZ=4.684; 52 | 53 | front = wb_robot_get_device("frontGps"); 54 | center = wb_robot_get_device("centerGps"); 55 | wb_gps_enable(front,time_step); 56 | wb_gps_enable(center,time_step); 57 | // set servos' positions 58 | wb_servo_set_position(leftWheel,INFINITY); 59 | wb_servo_set_position(rightWheel,INFINITY); 60 | // set speeds 61 | wb_servo_set_velocity(leftWheel, 0.0); 62 | wb_servo_set_velocity(rightWheel, 0.0); 63 | //perform one control loop 64 | wb_robot_step(time_step); 65 | distance_sensor[0] = wb_robot_get_device("so0"); 66 | distance_sensor[1] = wb_robot_get_device("so1"); 67 | distance_sensor[2] = wb_robot_get_device("so2"); 68 | distance_sensor[3] = wb_robot_get_device("so3"); 69 | distance_sensor[4] = wb_robot_get_device("so4"); 70 | distance_sensor[5] = wb_robot_get_device("so5"); 71 | distance_sensor[6] = wb_robot_get_device("so6"); 72 | distance_sensor[7] = wb_robot_get_device("so7"); 73 | wb_distance_sensor_enable(distance_sensor[0],time_step); 74 | wb_distance_sensor_enable(distance_sensor[1],time_step); 75 | wb_distance_sensor_enable(distance_sensor[2],time_step); 76 | wb_distance_sensor_enable(distance_sensor[3],time_step); 77 | wb_distance_sensor_enable(distance_sensor[4],time_step); 78 | wb_distance_sensor_enable(distance_sensor[5],time_step); 79 | wb_distance_sensor_enable(distance_sensor[6],time_step); 80 | wb_distance_sensor_enable(distance_sensor[7],time_step); 81 | // init dynamic variables 82 | double leftObstacle = 0.0, rightObstacle = 0.0, obstacle = 0.0; 83 | double deltaObstacle = 0.0; 84 | double leftSpeed = CRUISING_SPEED, rightSpeed = CRUISING_SPEED; 85 | double speedFactor = 1.0; 86 | float targetAngle; 87 | float currentAngle; 88 | float value = 0.0; 89 | int i = 0; 90 | double sideObstacle = 0.0; 91 | while (1) { 92 | 93 | for(i=0; i<8; i++){ 94 | distance_value[i] = wb_distance_sensor_get_value(distance_sensor[i]); 95 | } 96 | for(i=0; i<8; i++){ 97 | printf(" %f",distance_value[i]); 98 | } 99 | sideObstacle = distance_value[0] + distance_value[1] + distance_value[6] + distance_value[7]; 100 | 101 | 102 | 103 | kinectValues = (float*) wb_camera_get_range_image(kinect); 104 | for (i = 0; i < halfWidth; i++) { 105 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, i, viewHeight); 106 | if(value < rangeThreshold) { 107 | leftObstacle += value; 108 | } 109 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, kinectWidth - i, viewHeight); 110 | if(value < rangeThreshold) { 111 | rightObstacle += value; 112 | } 113 | } 114 | obstacle = leftObstacle + rightObstacle; 115 | point1 = (float*) wb_gps_get_values(front); 116 | point2 = (float*) wb_gps_get_values(center); 117 | targetAngle = atan2(targetZ-point1[2],targetX-point1[0]); 118 | currentAngle = atan2(point2[2]-point1[2],point2[0]-point1[0]); 119 | 120 | //if reach the target, stop 121 | if(sqrt(pow(targetZ-point2[2],2) + pow(targetX-point2[0],2))<0.6) 122 | { 123 | wb_servo_set_velocity(leftWheel, 0); 124 | wb_servo_set_velocity(rightWheel, 0); 125 | printf("!!! Target reached !!!"); 126 | break; 127 | } 128 | 129 | 130 | if(obstacle > 9){ 131 | printf("\n 1 \n"); 132 | obstacle = 1.0 - obstacle * invMaxRangeTimesWidth;// compute the relevant overall quantity of obstacle 133 | speedFactor = (obstacle > OBSTACLE_THRESHOLD) ? 0.0 : SLOWDOWN_FACTOR; 134 | deltaObstacle = - (leftObstacle - rightObstacle) * invMaxRangeTimesWidth; 135 | if(deltaObstacle > TOLERANCE){ 136 | leftSpeed = CRUISING_SPEED; 137 | rightSpeed = speedFactor * CRUISING_SPEED; 138 | } 139 | else { 140 | leftSpeed = speedFactor * CRUISING_SPEED; 141 | rightSpeed = CRUISING_SPEED; 142 | } 143 | }else if(sideObstacle < 0.1 &&fabs(targetAngle-currentAngle)>0.3){ 144 | printf("\n case: 2 \n"); 145 | if(targetAngle < currentAngle){ 146 | leftSpeed -=0.5; 147 | rightSpeed +=0.5; 148 | } 149 | else { 150 | leftSpeed +=0.5; 151 | rightSpeed -=0.5; 152 | } 153 | printf("x1:%2f z1:%2f; x2:%2f z2:%2f ", point1[0],point1[2],point2[0],point2[2]); 154 | }else { 155 | printf("3 \n"); 156 | leftSpeed = CRUISING_SPEED; 157 | rightSpeed = CRUISING_SPEED; 158 | } 159 | // set speeds 160 | wb_servo_set_velocity(leftWheel, leftSpeed); 161 | wb_servo_set_velocity(rightWheel, rightSpeed); 162 | printf("%2f, %2f \n",leftObstacle,rightObstacle); 163 | // run simulation 164 | wb_robot_step(time_step); 165 | leftObstacle = 0.0; 166 | rightObstacle = 0.0; 167 | } 168 | } 169 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.d: -------------------------------------------------------------------------------- 1 | obstacle_avoidance_with_kinect.o: obstacle_avoidance_with_kinect.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/servo.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/camera.h \ 6 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/distance_sensor.h \ 7 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/gps.h 8 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/supervisor.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define XCOORD 0 6 | #define ZCOORD 2 7 | 8 | WbNodeRef robot; 9 | WbFieldRef robot_translation_field; 10 | const double *robot_translation; 11 | static int time_step = 64; 12 | 13 | 14 | int main() { 15 | wb_robot_init(); 16 | //time_step = wb_robot_get_basic_time_step(); 17 | robot = wb_supervisor_node_get_from_def("PIONEER_3DX"); 18 | robot_translation_field = wb_supervisor_node_get_field(robot,"translation"); 19 | wb_robot_step(time_step); 20 | while(1){ 21 | robot_translation = wb_supervisor_field_get_sf_vec3f(robot_translation_field); 22 | wb_robot_step(time_step); 23 | printf("Pioneer 3DX is at: (%f, %f)\n",robot_translation[XCOORD],robot_translation[ZCOORD]); 24 | wb_robot_step(time_step); 25 | } 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/supervisor.d: -------------------------------------------------------------------------------- 1 | supervisor.o: supervisor.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/supervisor.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/nodes.h 6 | -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/supervisor.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/supervisor/supervisor.exe -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/supervisor.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/supervisor/supervisor.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug0/controllers/supervisor/supervisor.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/controllers/supervisor/supervisor.o -------------------------------------------------------------------------------- /pioneer3dx bug0/protos/Pioneer_DistanceSensor.proto: -------------------------------------------------------------------------------- 1 | PROTO Pioneer_DistanceSensor [ 2 | field SFVec3f translation 0 0.114 0 3 | field SFRotation rotation 0 1 0 0 4 | field SFString name "so" 5 | field SFString type "infra-red" 6 | ] 7 | { 8 | DistanceSensor { 9 | translation IS translation 10 | rotation IS rotation 11 | name IS name 12 | type IS type 13 | children [ 14 | DEF BLACK_BODY_SHAPE Shape { 15 | appearance Appearance { 16 | material Material { 17 | diffuseColor 0 0 0 18 | shininess 0.0976563 19 | specularColor 0.401201 0.401201 0.401201 20 | } 21 | } 22 | geometry IndexedFaceSet { 23 | coord Coordinate { 24 | point [ 25 | 0.0027176 -0.0082813 -0.0199851 26 | 0.0027176 -0.0152989 -0.0152947 27 | 0.0027176 -0.0199874 -0.0082758 28 | 0.0027176 -0.021633 3e-06 29 | 0.0027176 -0.0199851 0.0082813 30 | 0.0027176 -0.0152947 0.0152989 31 | 0.0027176 -0.0082758 0.0199874 32 | 0.0027176 3e-06 0.021633 33 | 0.0027176 0.0082813 0.0199851 34 | 0.0027176 0.0152989 0.0152947 35 | 0.0027176 0.0199874 0.0082758 36 | 0.0027176 0.021633 -3e-06 37 | 0.0027176 0.0199851 -0.0082813 38 | 0.0027176 0.0152947 -0.0152989 39 | 0.0027176 0.0082758 -0.0199874 40 | 0.0027176 -3e-06 -0.021633 41 | 3.68e-05 -0.0082813 -0.0199851 42 | 3.68e-05 -0.0152989 -0.0152947 43 | 3.68e-05 -0.0199874 -0.0082758 44 | 3.68e-05 -0.021633 3e-06 45 | 3.68e-05 -0.0199851 0.0082813 46 | 3.68e-05 -0.0152947 0.0152989 47 | 3.68e-05 -0.0082758 0.0199874 48 | 3.68e-05 3e-06 0.021633 49 | 3.68e-05 0.0082813 0.0199851 50 | 3.68e-05 0.0152989 0.0152947 51 | 3.68e-05 0.0199874 0.0082758 52 | 3.68e-05 0.021633 -3e-06 53 | 3.68e-05 0.0199851 -0.0082813 54 | 3.68e-05 0.0152947 -0.0152989 55 | 3.68e-05 0.0082758 -0.0199874 56 | 3.68e-05 -3e-06 -0.021633 57 | 0.0027176 -0.0071569 -0.0172717 58 | 0.0027176 -0.0132217 -0.0132181 59 | 0.0027176 -0.0172736 -0.0071522 60 | 0.0027176 -0.0186958 2.6e-06 61 | 0.0027176 -0.0172717 0.0071569 62 | 0.0027176 -0.0132181 0.0132217 63 | 0.0027176 -0.0071522 0.0172736 64 | 0.0027176 2.6e-06 0.0186958 65 | 0.0027176 0.0071569 0.0172717 66 | 0.0027176 0.0132217 0.0132181 67 | 0.0027176 0.0172736 0.0071522 68 | 0.0027176 0.0186958 -2.6e-06 69 | 0.0027176 0.0172717 -0.0071569 70 | 0.0027176 0.0132181 -0.0132217 71 | 0.0027176 0.0071522 -0.0172736 72 | 0.0027176 -2.6e-06 -0.0186958 73 | 0.0004008 -0.0071569 -0.0172717 74 | 0.0004008 -0.0132217 -0.0132181 75 | 0.0004008 -0.0172736 -0.0071522 76 | 0.0004008 -0.0186958 2.6e-06 77 | 0.0004008 -0.0172717 0.0071569 78 | 0.0004008 -0.0132181 0.0132217 79 | 0.0004008 -0.0071522 0.0172736 80 | 0.0004008 2.6e-06 0.0186958 81 | 0.0004008 0.0071569 0.0172717 82 | 0.0004008 0.0132217 0.0132181 83 | 0.0004008 0.0172736 0.0071522 84 | 0.0004008 0.0186958 -2.6e-06 85 | 0.0004008 0.0172717 -0.0071569 86 | 0.0004008 0.0132181 -0.0132217 87 | 0.0004008 0.0071522 -0.0172736 88 | 0.0004008 -2.6e-06 -0.0186958 89 | ] 90 | } 91 | coordIndex [ 92 | 63, 46, 62, -1, 63, 47, 46, -1, 61, 62, 93 | 46, -1, 61, 46, 45, -1, 60, 61, 45, -1, 94 | 60, 45, 44, -1, 60, 44, 43, -1, 60, 43, 95 | 59, -1, 59, 43, 42, -1, 59, 42, 58, -1, 96 | 57, 58, 42, -1, 57, 42, 41, -1, 56, 57, 97 | 41, -1, 56, 41, 40, -1, 56, 40, 39, -1, 98 | 56, 39, 55, -1, 55, 39, 38, -1, 55, 38, 99 | 54, -1, 53, 54, 38, -1, 53, 38, 37, -1, 100 | 52, 53, 37, -1, 52, 37, 36, -1, 52, 36, 101 | 35, -1, 52, 35, 51, -1, 51, 35, 34, -1, 102 | 51, 34, 50, -1, 49, 50, 34, -1, 49, 34, 103 | 33, -1, 49, 33, 32, -1, 49, 32, 48, -1, 104 | 15, 14, 46, -1, 15, 46, 47, -1, 13, 45, 105 | 46, -1, 13, 46, 14, -1, 12, 44, 45, -1, 106 | 12, 45, 13, -1, 12, 11, 43, -1, 12, 43, 107 | 44, -1, 11, 10, 42, -1, 11, 42, 43, -1, 108 | 9, 41, 42, -1, 9, 42, 10, -1, 8, 40, 109 | 41, -1, 8, 41, 9, -1, 8, 7, 39, -1, 110 | 8, 39, 40, -1, 7, 6, 38, -1, 7, 38, 111 | 39, -1, 5, 37, 38, -1, 5, 38, 6, -1, 112 | 4, 36, 37, -1, 4, 37, 5, -1, 4, 3, 113 | 35, -1, 4, 35, 36, -1, 3, 2, 34, -1, 114 | 3, 34, 35, -1, 1, 33, 34, -1, 1, 34, 115 | 2, -1, 1, 0, 32, -1, 1, 32, 33, -1, 116 | 15, 47, 32, -1, 32, 0, 15, -1, 14, 15, 117 | 31, -1, 14, 31, 30, -1, 14, 30, 29, -1, 118 | 14, 29, 13, -1, 13, 29, 28, -1, 13, 28, 119 | 12, -1, 11, 12, 28, -1, 11, 28, 27, -1, 120 | 10, 11, 27, -1, 10, 27, 26, -1, 10, 26, 121 | 25, -1, 10, 25, 9, -1, 9, 25, 24, -1, 122 | 9, 24, 8, -1, 7, 8, 24, -1, 7, 24, 123 | 23, -1, 6, 7, 23, -1, 6, 23, 22, -1, 124 | 6, 22, 21, -1, 6, 21, 5, -1, 5, 21, 125 | 20, -1, 5, 20, 4, -1, 3, 4, 20, -1, 126 | 3, 20, 19, -1, 2, 3, 19, -1, 2, 19, 127 | 18, -1, 2, 18, 17, -1, 2, 17, 1, -1, 128 | 0, 1, 17, -1, 0, 17, 16, -1, 0, 16, 129 | 31, -1, 0, 31, 15, -1, 47, 48, 32, -1, 130 | 47, 63, 48, -1 131 | ] 132 | creaseAngle 1 133 | } 134 | } 135 | DEF BOTTOM_DISC_SHAPE Shape { 136 | appearance Appearance { 137 | material Material { 138 | shininess 0.0976563 139 | specularColor 0.401201 0.401201 0.401201 140 | } 141 | texture ImageTexture { 142 | url [ 143 | "textures/honeycomb.png" 144 | ] 145 | } 146 | } 147 | geometry IndexedFaceSet { 148 | coord DEF coord_BOTTOM_DISC Coordinate { 149 | point [ 150 | 0.0001602 3.59e-05 0.0196526 151 | 0.0001602 -0.0075108 0.0181533 152 | 0.0001602 -0.0139092 0.0138801 153 | 0.0001602 -0.0181854 0.0074836 154 | 0.0001602 0.0075819 0.0181497 155 | 0.0001602 0.0139783 0.0138736 156 | 0.0001602 0.0182515 0.0074751 157 | 0.0001602 0.0197508 -7.15e-05 158 | 0.0001602 0.018248 -0.0076175 159 | 0.0001602 0.0139718 -0.014014 160 | 0.0001602 0.0075734 -0.0182871 161 | 0.0001602 2.67e-05 -0.0197864 162 | 0.0001602 -0.0075193 -0.0182836 163 | 0.0001602 -0.0139157 -0.0140075 164 | 0.0001602 -0.0181889 -0.007609 165 | 0.0001602 -0.0196882 -6.23e-05 166 | ] 167 | } 168 | texCoord TextureCoordinate { 169 | point [ 170 | 0.404999 0.9776 171 | 0.229461 0.90489 172 | 0.09511 0.770539 173 | 0.404999 0.9776 174 | 0.09511 0.770539 175 | 0.0223996 0.595001 176 | 0.595 0.9776 177 | 0.404999 0.9776 178 | 0.0223996 0.595001 179 | 0.595 0.9776 180 | 0.0223996 0.595001 181 | 0.0223995 0.405 182 | 0.770539 0.90489 183 | 0.595 0.9776 184 | 0.0223995 0.405 185 | 0.0223995 0.405 186 | 0.0951098 0.229461 187 | 0.770539 0.90489 188 | 0.770539 0.90489 189 | 0.0951098 0.229461 190 | 0.90489 0.770539 191 | 0.90489 0.770539 192 | 0.0951098 0.229461 193 | 0.229461 0.09511 194 | 0.9776 0.595001 195 | 0.90489 0.770539 196 | 0.229461 0.09511 197 | 0.9776 0.595001 198 | 0.229461 0.09511 199 | 0.405 0.0223995 200 | 0.9776 0.405 201 | 0.9776 0.595001 202 | 0.405 0.0223995 203 | 0.9776 0.405 204 | 0.405 0.0223995 205 | 0.595001 0.0223997 206 | 0.90489 0.229462 207 | 0.9776 0.405 208 | 0.595001 0.0223997 209 | 0.90489 0.229462 210 | 0.595001 0.0223997 211 | 0.770539 0.0951104 212 | ] 213 | } 214 | coordIndex [ 215 | 8, 7, 6, -1, 8, 6, 5, -1, 9, 8, 216 | 5, -1, 9, 5, 4, -1, 10, 9, 4, -1, 217 | 4, 0, 10, -1, 10, 0, 11, -1, 11, 0, 218 | 1, -1, 12, 11, 1, -1, 12, 1, 2, -1, 219 | 13, 12, 2, -1, 13, 2, 3, -1, 14, 13, 220 | 3, -1, 14, 3, 15, -1 221 | ] 222 | texCoordIndex [ 223 | 0, 1, 2, -1, 3, 4, 5, -1, 6, 7, 224 | 8, -1, 9, 10, 11, -1, 12, 13, 14, -1, 225 | 15, 16, 17, -1, 18, 19, 20, -1, 21, 22, 226 | 23, -1, 24, 25, 26, -1, 27, 28, 29, -1, 227 | 30, 31, 32, -1, 33, 34, 35, -1, 36, 37, 228 | 38, -1, 39, 40, 41, -1 229 | ] 230 | creaseAngle 1 231 | } 232 | } 233 | DEF SONAR_GLASS_SHAPE Shape { 234 | appearance Appearance { 235 | material Material { 236 | diffuseColor 1 0.995646 1 237 | shininess 0.0976563 238 | specularColor 0.401201 0.401201 0.401201 239 | transparency 0.699218 240 | } 241 | } 242 | geometry IndexedFaceSet { 243 | coord Coordinate { 244 | point [ 245 | 0.0022441 -0.0078844 -0.017925 246 | 0.0022441 -0.014335 -0.0136148 247 | 0.0022441 -0.0186452 -0.0071641 248 | 0.0022441 -0.0201588 0.0004449 249 | 0.0022441 -0.0186452 0.008054 250 | 0.0022441 -0.014335 0.0145046 251 | 0.0022441 -0.0078844 0.0188148 252 | 0.0022441 -0.0002753 0.0203284 253 | 0.0022441 0.0073338 0.0188148 254 | 0.0022441 0.0137844 0.0145046 255 | 0.0022441 0.0180946 0.008054 256 | 0.0022441 0.0196081 0.0004449 257 | 0.0022441 0.0180946 -0.0071642 258 | 0.0022441 0.0137844 -0.0136148 259 | 0.0022441 0.0073337 -0.017925 260 | 0.0022441 -0.0002753 -0.0194385 261 | ] 262 | } 263 | coordIndex [ 264 | 12, 11, 10, -1, 12, 10, 9, -1, 13, 12, 265 | 9, -1, 13, 9, 8, -1, 14, 13, 8, -1, 266 | 14, 8, 7, -1, 15, 14, 7, -1, 15, 7, 267 | 6, -1, 0, 15, 6, -1, 0, 6, 5, -1, 268 | 1, 0, 5, -1, 1, 5, 4, -1, 2, 1, 269 | 4, -1, 2, 4, 3, -1 270 | ] 271 | creaseAngle 1 272 | } 273 | } 274 | ] 275 | lookupTable [ 276 | 0 1024 0 277 | 0.2 1024 0.1 278 | 0.4 0 0.1 279 | ] 280 | } 281 | } 282 | -------------------------------------------------------------------------------- /pioneer3dx bug0/protos/textures/honeycomb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/protos/textures/honeycomb.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/.pioneer3dx_with_kinect.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version 7.0 2 | perspectives: 000000ff00000000fd00000003000000000000013a00000293fc0200000001fb00000012005300630065006e00650054007200650065010000001500000293000000f100ffffff0000000100000203000001befc0200000001fb0000001400540065007800740045006400690074006f00720100000015000001be0000009600ffffff0000000300000406000000cffc0100000001fb0000000e0043006f006e0073006f006c00650100000140000004060000004700ffffff000001fd000001be00000001000000020000000100000008fc00000000 3 | minimizedPerspectives: 4 | maximizedDockId: -1 5 | centralWidgetVisible: 1 6 | textFiles: 0 "controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c" 7 | -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/.pioneer3dx_with_kinect.wbt.project: -------------------------------------------------------------------------------- 1 | Webots Project File version 6.0 2 | perspectives: 0 "Default" "layout2|name=3D view;caption=3D view;state=4196348;dir=5;layer=0;row=0;pos=0;prop=100000;bestw=0.951684%;besth=2.08024%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=text editor;caption=Text editor;state=6309884;dir=2;layer=0;row=0;pos=0;prop=100000;bestw=35.2855%;besth=62.8529%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=console;caption=Console;state=6293500;dir=3;layer=0;row=1;pos=0;prop=100000;bestw=89.7511%;besth=20.6538%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=351;floaty=531;floatw=400;floath=492|name=scene tree;caption=Scene tree;state=6293500;dir=4;layer=1;row=0;pos=0;prop=100000;bestw=17.716%;besth=83.9525%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|dock_size(5,0,0)=2.70864%|dock_size(4,1,0)=12.3719%|dock_size(2,0,0)=43.3382%|dock_size(3,0,1)=3.73353%|" 3 | textFiles: 0 "controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c" 4 | -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/body_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/body_3dx.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/caster_wheel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/caster_wheel.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/parquet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/parquet.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/serial_plug.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/serial_plug.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/smooth_rubber.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/smooth_rubber.png -------------------------------------------------------------------------------- /pioneer3dx bug0/worlds/textures/top_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug0/worlds/textures/top_3dx.png -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/.obstacle_avoidance_with_kinect.exe.signature: -------------------------------------------------------------------------------- 1 | 15532e9c-c2e75a40-19fb921e-94b080e5-81eb4c0a 2 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c: -------------------------------------------------------------------------------- 1 | /* 2 | * File: obstacle_avoidance_with_kinect.c 3 | * Date: August 24th, 2011 4 | * Description: A Braitenberg-like controller moving a Pioneer 3-DX equipped with a kinect. 5 | * Author: can 6 | * 7 | * Copyright (c) 2011 Cyberbotics - www.cyberbotics.com 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #define MAX_SPEED 10.0 22 | #define CRUISING_SPEED 7.5 23 | #define TOLERANCE -0.1 24 | #define OBSTACLE_THRESHOLD 0.5 25 | #define SLOWDOWN_FACTOR 0.5 26 | 27 | #define FOLLOW_OBSTACLE 1 28 | #define GO_TO_TARGET 2 29 | 30 | static WbDeviceTag backGPS, centerGPS; 31 | static WbDeviceTag leftWheel, rightWheel; 32 | static WbDeviceTag kinect; 33 | const float* kinectValues; 34 | static int time_step = 64; 35 | const double *point1; 36 | const double *point2; 37 | static WbDeviceTag distance_sensor[8]; 38 | double distance_value[8]; 39 | int main() 40 | { 41 | wb_robot_init(); 42 | time_step = wb_robot_get_basic_time_step(); 43 | leftWheel = wb_robot_get_device("leftWheel"); 44 | rightWheel = wb_robot_get_device("rightWheel"); 45 | kinect = wb_robot_get_device("kinect"); 46 | wb_camera_enable(kinect, time_step); 47 | int kinectWidth = wb_camera_get_width(kinect); 48 | int kinectHeight = wb_camera_get_height(kinect); 49 | int halfWidth = kinectWidth/2; 50 | int viewHeight = kinectHeight/2 + 10; 51 | double maxRange = wb_camera_get_max_range(kinect); 52 | double rangeThreshold = 0.7; 53 | double invMaxRangeTimesWidth = 1.0 / (maxRange * kinectWidth); 54 | static float targetX=0; 55 | static float targetZ=4.684; 56 | 57 | 58 | backGPS = wb_robot_get_device("frontGps"); //name is wrong, it is actually at back 59 | centerGPS = wb_robot_get_device("centerGps"); 60 | wb_gps_enable(backGPS,time_step); 61 | wb_gps_enable(centerGPS,time_step); 62 | // set servos' positions 63 | wb_servo_set_position(leftWheel,INFINITY); 64 | wb_servo_set_position(rightWheel,INFINITY); 65 | // set speeds 66 | wb_servo_set_velocity(leftWheel, 0.0); 67 | wb_servo_set_velocity(rightWheel, 0.0); 68 | //perform one control loop 69 | wb_robot_step(time_step); 70 | distance_sensor[0] = wb_robot_get_device("so0"); 71 | distance_sensor[1] = wb_robot_get_device("so1"); 72 | distance_sensor[2] = wb_robot_get_device("so2"); 73 | distance_sensor[3] = wb_robot_get_device("so3"); 74 | distance_sensor[4] = wb_robot_get_device("so4"); 75 | distance_sensor[5] = wb_robot_get_device("so5"); 76 | distance_sensor[6] = wb_robot_get_device("so6"); 77 | distance_sensor[7] = wb_robot_get_device("so7"); 78 | wb_distance_sensor_enable(distance_sensor[0],time_step); 79 | wb_distance_sensor_enable(distance_sensor[1],time_step); 80 | wb_distance_sensor_enable(distance_sensor[2],time_step); 81 | wb_distance_sensor_enable(distance_sensor[3],time_step); 82 | wb_distance_sensor_enable(distance_sensor[4],time_step); 83 | wb_distance_sensor_enable(distance_sensor[5],time_step); 84 | wb_distance_sensor_enable(distance_sensor[6],time_step); 85 | wb_distance_sensor_enable(distance_sensor[7],time_step); 86 | 87 | // init dynamic variables 88 | double leftObstacle = 0.0, rightObstacle = 0.0, obstacle = 0.0; 89 | double deltaObstacle = 0.0; 90 | double leftSpeed = CRUISING_SPEED, rightSpeed = CRUISING_SPEED; 91 | double speedFactor = 1.0; 92 | float targetAngle; 93 | float currentAngle; 94 | float value = 0.0; 95 | int i = 0; 96 | double sideObstacle = 0.0; 97 | int mode=GO_TO_TARGET; 98 | float distanceToTarget; 99 | 100 | //saving the closed position to target and distance value 101 | float minDistanceX; 102 | float minDistanceZ; 103 | float minDistanceToTarget; 104 | time_t minDistanceTime,currentTime; 105 | 106 | while (1) { 107 | 108 | //detector sampling... 109 | for(i=0; i<8; i++){ 110 | distance_value[i] = wb_distance_sensor_get_value(distance_sensor[i]); 111 | } 112 | sideObstacle = distance_value[0] + distance_value[1] + distance_value[6] + distance_value[7]; 113 | 114 | kinectValues = (float*) wb_camera_get_range_image(kinect); 115 | for (i = 0; i < halfWidth; i++) { 116 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, i, viewHeight); 117 | if(value < rangeThreshold) { 118 | leftObstacle += value; 119 | } 120 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, kinectWidth - i, viewHeight); 121 | if(value < rangeThreshold) { 122 | rightObstacle += value; 123 | } 124 | } 125 | obstacle = leftObstacle + rightObstacle; 126 | point1 = (float*) wb_gps_get_values(backGPS); 127 | point2 = (float*) wb_gps_get_values(centerGPS); 128 | 129 | targetAngle = atan2(targetZ-point1[2],targetX-point1[0]); 130 | currentAngle = atan2(point2[2]-point1[2],point2[0]-point1[0]); 131 | 132 | //if reach the target, stop 133 | distanceToTarget=sqrt(pow(targetZ-point2[2],2) + pow(targetX-point2[0],2)); 134 | if(distanceToTarget<0.6) 135 | { 136 | wb_servo_set_velocity(leftWheel, 0); 137 | wb_servo_set_velocity(rightWheel, 0); 138 | printf("\n\n\n!!! Target reached !!!\n"); 139 | break; 140 | } 141 | 142 | switch(mode){ 143 | case FOLLOW_OBSTACLE: 144 | printf("FOLLOW_OBSTACLE \n"); 145 | sqrt(pow(targetZ-point2[2],2) + pow(targetX-point2[0],2)); 146 | if(minDistanceToTarget> distanceToTarget) 147 | { 148 | time(&minDistanceTime); 149 | minDistanceToTarget = distanceToTarget; 150 | minDistanceX = point2[0]; 151 | minDistanceZ = point2[2]; 152 | } 153 | else if(sqrt(pow(minDistanceZ-point2[2],2) + pow(minDistanceX-point2[0],2)<0.01)) 154 | { 155 | time(¤tTime); 156 | if(difftime (currentTime,minDistanceTime) > 2.00) 157 | mode=GO_TO_TARGET; 158 | } 159 | if(obstacle > 3.0) 160 | { 161 | obstacle = 1.0 - obstacle * invMaxRangeTimesWidth;// compute the relevant overall quantity of obstacle 162 | speedFactor = (obstacle > OBSTACLE_THRESHOLD) ? 0.0 : SLOWDOWN_FACTOR; 163 | leftSpeed = CRUISING_SPEED; 164 | rightSpeed = speedFactor * CRUISING_SPEED; 165 | }else if(distance_value[0]<1.0){ 166 | leftSpeed -=1; 167 | rightSpeed +=1; 168 | }else{ 169 | leftSpeed = CRUISING_SPEED; 170 | rightSpeed = CRUISING_SPEED; 171 | } 172 | break; 173 | case GO_TO_TARGET: 174 | printf("GO_TO_TARGET \n"); 175 | if(fabs(targetAngle-currentAngle)>0.3){ 176 | if(targetAngle < currentAngle){ 177 | leftSpeed -=0.5; 178 | rightSpeed +=0.5; 179 | } 180 | else { 181 | leftSpeed +=0.5; 182 | rightSpeed -=0.5; 183 | } 184 | }else { 185 | printf("CRUISING_SPEED \n"); 186 | leftSpeed = CRUISING_SPEED; 187 | rightSpeed = CRUISING_SPEED; 188 | } 189 | if(obstacle > 5.0) 190 | { 191 | time(&minDistanceTime); 192 | minDistanceX = point2[0]; 193 | minDistanceZ = point2[2]; 194 | minDistanceToTarget = distanceToTarget; 195 | mode=FOLLOW_OBSTACLE; 196 | } 197 | break; 198 | } 199 | // set speeds 200 | wb_servo_set_velocity(leftWheel, leftSpeed); 201 | wb_servo_set_velocity(rightWheel, rightSpeed); 202 | printf("%2f, %2f \n",leftObstacle,rightObstacle); 203 | printf("minDistance x:%2f, z:%2f \n", minDistanceX, minDistanceZ); 204 | // run simulation 205 | wb_robot_step(time_step); 206 | leftObstacle = 0.0; 207 | rightObstacle = 0.0; 208 | } 209 | } 210 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.d: -------------------------------------------------------------------------------- 1 | obstacle_avoidance_with_kinect.o: obstacle_avoidance_with_kinect.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/servo.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/camera.h \ 6 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/distance_sensor.h \ 7 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/gps.h 8 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/supervisor.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define XCOORD 0 6 | #define ZCOORD 2 7 | 8 | WbNodeRef robot; 9 | WbFieldRef robot_translation_field; 10 | const double *robot_translation; 11 | static int time_step = 64; 12 | 13 | 14 | int main() { 15 | wb_robot_init(); 16 | //time_step = wb_robot_get_basic_time_step(); 17 | robot = wb_supervisor_node_get_from_def("PIONEER_3DX"); 18 | robot_translation_field = wb_supervisor_node_get_field(robot,"translation"); 19 | wb_robot_step(time_step); 20 | while(1){ 21 | robot_translation = wb_supervisor_field_get_sf_vec3f(robot_translation_field); 22 | wb_robot_step(time_step); 23 | printf("Pioneer 3DX is at: (%f, %f)\n",robot_translation[XCOORD],robot_translation[ZCOORD]); 24 | wb_robot_step(time_step); 25 | } 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/supervisor.d: -------------------------------------------------------------------------------- 1 | supervisor.o: supervisor.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/supervisor.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/nodes.h 6 | -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/supervisor.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/supervisor/supervisor.exe -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/supervisor.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/supervisor/supervisor.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug1/controllers/supervisor/supervisor.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/controllers/supervisor/supervisor.o -------------------------------------------------------------------------------- /pioneer3dx bug1/protos/Pioneer_DistanceSensor.proto: -------------------------------------------------------------------------------- 1 | PROTO Pioneer_DistanceSensor [ 2 | field SFVec3f translation 0 0.114 0 3 | field SFRotation rotation 0 1 0 0 4 | field SFString name "so" 5 | field SFString type "infra-red" 6 | ] 7 | { 8 | DistanceSensor { 9 | translation IS translation 10 | rotation IS rotation 11 | name IS name 12 | type IS type 13 | children [ 14 | DEF BLACK_BODY_SHAPE Shape { 15 | appearance Appearance { 16 | material Material { 17 | diffuseColor 0 0 0 18 | shininess 0.0976563 19 | specularColor 0.401201 0.401201 0.401201 20 | } 21 | } 22 | geometry IndexedFaceSet { 23 | coord Coordinate { 24 | point [ 25 | 0.0027176 -0.0082813 -0.0199851 26 | 0.0027176 -0.0152989 -0.0152947 27 | 0.0027176 -0.0199874 -0.0082758 28 | 0.0027176 -0.021633 3e-06 29 | 0.0027176 -0.0199851 0.0082813 30 | 0.0027176 -0.0152947 0.0152989 31 | 0.0027176 -0.0082758 0.0199874 32 | 0.0027176 3e-06 0.021633 33 | 0.0027176 0.0082813 0.0199851 34 | 0.0027176 0.0152989 0.0152947 35 | 0.0027176 0.0199874 0.0082758 36 | 0.0027176 0.021633 -3e-06 37 | 0.0027176 0.0199851 -0.0082813 38 | 0.0027176 0.0152947 -0.0152989 39 | 0.0027176 0.0082758 -0.0199874 40 | 0.0027176 -3e-06 -0.021633 41 | 3.68e-05 -0.0082813 -0.0199851 42 | 3.68e-05 -0.0152989 -0.0152947 43 | 3.68e-05 -0.0199874 -0.0082758 44 | 3.68e-05 -0.021633 3e-06 45 | 3.68e-05 -0.0199851 0.0082813 46 | 3.68e-05 -0.0152947 0.0152989 47 | 3.68e-05 -0.0082758 0.0199874 48 | 3.68e-05 3e-06 0.021633 49 | 3.68e-05 0.0082813 0.0199851 50 | 3.68e-05 0.0152989 0.0152947 51 | 3.68e-05 0.0199874 0.0082758 52 | 3.68e-05 0.021633 -3e-06 53 | 3.68e-05 0.0199851 -0.0082813 54 | 3.68e-05 0.0152947 -0.0152989 55 | 3.68e-05 0.0082758 -0.0199874 56 | 3.68e-05 -3e-06 -0.021633 57 | 0.0027176 -0.0071569 -0.0172717 58 | 0.0027176 -0.0132217 -0.0132181 59 | 0.0027176 -0.0172736 -0.0071522 60 | 0.0027176 -0.0186958 2.6e-06 61 | 0.0027176 -0.0172717 0.0071569 62 | 0.0027176 -0.0132181 0.0132217 63 | 0.0027176 -0.0071522 0.0172736 64 | 0.0027176 2.6e-06 0.0186958 65 | 0.0027176 0.0071569 0.0172717 66 | 0.0027176 0.0132217 0.0132181 67 | 0.0027176 0.0172736 0.0071522 68 | 0.0027176 0.0186958 -2.6e-06 69 | 0.0027176 0.0172717 -0.0071569 70 | 0.0027176 0.0132181 -0.0132217 71 | 0.0027176 0.0071522 -0.0172736 72 | 0.0027176 -2.6e-06 -0.0186958 73 | 0.0004008 -0.0071569 -0.0172717 74 | 0.0004008 -0.0132217 -0.0132181 75 | 0.0004008 -0.0172736 -0.0071522 76 | 0.0004008 -0.0186958 2.6e-06 77 | 0.0004008 -0.0172717 0.0071569 78 | 0.0004008 -0.0132181 0.0132217 79 | 0.0004008 -0.0071522 0.0172736 80 | 0.0004008 2.6e-06 0.0186958 81 | 0.0004008 0.0071569 0.0172717 82 | 0.0004008 0.0132217 0.0132181 83 | 0.0004008 0.0172736 0.0071522 84 | 0.0004008 0.0186958 -2.6e-06 85 | 0.0004008 0.0172717 -0.0071569 86 | 0.0004008 0.0132181 -0.0132217 87 | 0.0004008 0.0071522 -0.0172736 88 | 0.0004008 -2.6e-06 -0.0186958 89 | ] 90 | } 91 | coordIndex [ 92 | 63, 46, 62, -1, 63, 47, 46, -1, 61, 62, 93 | 46, -1, 61, 46, 45, -1, 60, 61, 45, -1, 94 | 60, 45, 44, -1, 60, 44, 43, -1, 60, 43, 95 | 59, -1, 59, 43, 42, -1, 59, 42, 58, -1, 96 | 57, 58, 42, -1, 57, 42, 41, -1, 56, 57, 97 | 41, -1, 56, 41, 40, -1, 56, 40, 39, -1, 98 | 56, 39, 55, -1, 55, 39, 38, -1, 55, 38, 99 | 54, -1, 53, 54, 38, -1, 53, 38, 37, -1, 100 | 52, 53, 37, -1, 52, 37, 36, -1, 52, 36, 101 | 35, -1, 52, 35, 51, -1, 51, 35, 34, -1, 102 | 51, 34, 50, -1, 49, 50, 34, -1, 49, 34, 103 | 33, -1, 49, 33, 32, -1, 49, 32, 48, -1, 104 | 15, 14, 46, -1, 15, 46, 47, -1, 13, 45, 105 | 46, -1, 13, 46, 14, -1, 12, 44, 45, -1, 106 | 12, 45, 13, -1, 12, 11, 43, -1, 12, 43, 107 | 44, -1, 11, 10, 42, -1, 11, 42, 43, -1, 108 | 9, 41, 42, -1, 9, 42, 10, -1, 8, 40, 109 | 41, -1, 8, 41, 9, -1, 8, 7, 39, -1, 110 | 8, 39, 40, -1, 7, 6, 38, -1, 7, 38, 111 | 39, -1, 5, 37, 38, -1, 5, 38, 6, -1, 112 | 4, 36, 37, -1, 4, 37, 5, -1, 4, 3, 113 | 35, -1, 4, 35, 36, -1, 3, 2, 34, -1, 114 | 3, 34, 35, -1, 1, 33, 34, -1, 1, 34, 115 | 2, -1, 1, 0, 32, -1, 1, 32, 33, -1, 116 | 15, 47, 32, -1, 32, 0, 15, -1, 14, 15, 117 | 31, -1, 14, 31, 30, -1, 14, 30, 29, -1, 118 | 14, 29, 13, -1, 13, 29, 28, -1, 13, 28, 119 | 12, -1, 11, 12, 28, -1, 11, 28, 27, -1, 120 | 10, 11, 27, -1, 10, 27, 26, -1, 10, 26, 121 | 25, -1, 10, 25, 9, -1, 9, 25, 24, -1, 122 | 9, 24, 8, -1, 7, 8, 24, -1, 7, 24, 123 | 23, -1, 6, 7, 23, -1, 6, 23, 22, -1, 124 | 6, 22, 21, -1, 6, 21, 5, -1, 5, 21, 125 | 20, -1, 5, 20, 4, -1, 3, 4, 20, -1, 126 | 3, 20, 19, -1, 2, 3, 19, -1, 2, 19, 127 | 18, -1, 2, 18, 17, -1, 2, 17, 1, -1, 128 | 0, 1, 17, -1, 0, 17, 16, -1, 0, 16, 129 | 31, -1, 0, 31, 15, -1, 47, 48, 32, -1, 130 | 47, 63, 48, -1 131 | ] 132 | creaseAngle 1 133 | } 134 | } 135 | DEF BOTTOM_DISC_SHAPE Shape { 136 | appearance Appearance { 137 | material Material { 138 | shininess 0.0976563 139 | specularColor 0.401201 0.401201 0.401201 140 | } 141 | texture ImageTexture { 142 | url [ 143 | "textures/honeycomb.png" 144 | ] 145 | } 146 | } 147 | geometry IndexedFaceSet { 148 | coord DEF coord_BOTTOM_DISC Coordinate { 149 | point [ 150 | 0.0001602 3.59e-05 0.0196526 151 | 0.0001602 -0.0075108 0.0181533 152 | 0.0001602 -0.0139092 0.0138801 153 | 0.0001602 -0.0181854 0.0074836 154 | 0.0001602 0.0075819 0.0181497 155 | 0.0001602 0.0139783 0.0138736 156 | 0.0001602 0.0182515 0.0074751 157 | 0.0001602 0.0197508 -7.15e-05 158 | 0.0001602 0.018248 -0.0076175 159 | 0.0001602 0.0139718 -0.014014 160 | 0.0001602 0.0075734 -0.0182871 161 | 0.0001602 2.67e-05 -0.0197864 162 | 0.0001602 -0.0075193 -0.0182836 163 | 0.0001602 -0.0139157 -0.0140075 164 | 0.0001602 -0.0181889 -0.007609 165 | 0.0001602 -0.0196882 -6.23e-05 166 | ] 167 | } 168 | texCoord TextureCoordinate { 169 | point [ 170 | 0.404999 0.9776 171 | 0.229461 0.90489 172 | 0.09511 0.770539 173 | 0.404999 0.9776 174 | 0.09511 0.770539 175 | 0.0223996 0.595001 176 | 0.595 0.9776 177 | 0.404999 0.9776 178 | 0.0223996 0.595001 179 | 0.595 0.9776 180 | 0.0223996 0.595001 181 | 0.0223995 0.405 182 | 0.770539 0.90489 183 | 0.595 0.9776 184 | 0.0223995 0.405 185 | 0.0223995 0.405 186 | 0.0951098 0.229461 187 | 0.770539 0.90489 188 | 0.770539 0.90489 189 | 0.0951098 0.229461 190 | 0.90489 0.770539 191 | 0.90489 0.770539 192 | 0.0951098 0.229461 193 | 0.229461 0.09511 194 | 0.9776 0.595001 195 | 0.90489 0.770539 196 | 0.229461 0.09511 197 | 0.9776 0.595001 198 | 0.229461 0.09511 199 | 0.405 0.0223995 200 | 0.9776 0.405 201 | 0.9776 0.595001 202 | 0.405 0.0223995 203 | 0.9776 0.405 204 | 0.405 0.0223995 205 | 0.595001 0.0223997 206 | 0.90489 0.229462 207 | 0.9776 0.405 208 | 0.595001 0.0223997 209 | 0.90489 0.229462 210 | 0.595001 0.0223997 211 | 0.770539 0.0951104 212 | ] 213 | } 214 | coordIndex [ 215 | 8, 7, 6, -1, 8, 6, 5, -1, 9, 8, 216 | 5, -1, 9, 5, 4, -1, 10, 9, 4, -1, 217 | 4, 0, 10, -1, 10, 0, 11, -1, 11, 0, 218 | 1, -1, 12, 11, 1, -1, 12, 1, 2, -1, 219 | 13, 12, 2, -1, 13, 2, 3, -1, 14, 13, 220 | 3, -1, 14, 3, 15, -1 221 | ] 222 | texCoordIndex [ 223 | 0, 1, 2, -1, 3, 4, 5, -1, 6, 7, 224 | 8, -1, 9, 10, 11, -1, 12, 13, 14, -1, 225 | 15, 16, 17, -1, 18, 19, 20, -1, 21, 22, 226 | 23, -1, 24, 25, 26, -1, 27, 28, 29, -1, 227 | 30, 31, 32, -1, 33, 34, 35, -1, 36, 37, 228 | 38, -1, 39, 40, 41, -1 229 | ] 230 | creaseAngle 1 231 | } 232 | } 233 | DEF SONAR_GLASS_SHAPE Shape { 234 | appearance Appearance { 235 | material Material { 236 | diffuseColor 1 0.995646 1 237 | shininess 0.0976563 238 | specularColor 0.401201 0.401201 0.401201 239 | transparency 0.699218 240 | } 241 | } 242 | geometry IndexedFaceSet { 243 | coord Coordinate { 244 | point [ 245 | 0.0022441 -0.0078844 -0.017925 246 | 0.0022441 -0.014335 -0.0136148 247 | 0.0022441 -0.0186452 -0.0071641 248 | 0.0022441 -0.0201588 0.0004449 249 | 0.0022441 -0.0186452 0.008054 250 | 0.0022441 -0.014335 0.0145046 251 | 0.0022441 -0.0078844 0.0188148 252 | 0.0022441 -0.0002753 0.0203284 253 | 0.0022441 0.0073338 0.0188148 254 | 0.0022441 0.0137844 0.0145046 255 | 0.0022441 0.0180946 0.008054 256 | 0.0022441 0.0196081 0.0004449 257 | 0.0022441 0.0180946 -0.0071642 258 | 0.0022441 0.0137844 -0.0136148 259 | 0.0022441 0.0073337 -0.017925 260 | 0.0022441 -0.0002753 -0.0194385 261 | ] 262 | } 263 | coordIndex [ 264 | 12, 11, 10, -1, 12, 10, 9, -1, 13, 12, 265 | 9, -1, 13, 9, 8, -1, 14, 13, 8, -1, 266 | 14, 8, 7, -1, 15, 14, 7, -1, 15, 7, 267 | 6, -1, 0, 15, 6, -1, 0, 6, 5, -1, 268 | 1, 0, 5, -1, 1, 5, 4, -1, 2, 1, 269 | 4, -1, 2, 4, 3, -1 270 | ] 271 | creaseAngle 1 272 | } 273 | } 274 | ] 275 | lookupTable [ 276 | 0 1024 0 277 | 0.2 1024 0.1 278 | 0.4 0 0.1 279 | ] 280 | } 281 | } 282 | -------------------------------------------------------------------------------- /pioneer3dx bug1/protos/textures/honeycomb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/protos/textures/honeycomb.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/.pioneer3dx_with_kinect.wbt.project: -------------------------------------------------------------------------------- 1 | Webots Project File version 6.0 2 | perspectives: 0 "Default" "layout2|name=3D view;caption=3D view;state=4196348;dir=5;layer=0;row=0;pos=0;prop=100000;bestw=0.951684%;besth=2.08024%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=text editor;caption=Text editor;state=6309884;dir=2;layer=0;row=0;pos=0;prop=100000;bestw=35.2123%;besth=62.8529%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=console;caption=Console;state=6293500;dir=3;layer=0;row=1;pos=0;prop=100000;bestw=89.7511%;besth=20.6538%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=351;floaty=531;floatw=400;floath=492|name=scene tree;caption=Scene tree;state=6293500;dir=4;layer=1;row=0;pos=0;prop=100000;bestw=17.716%;besth=83.9525%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|dock_size(5,0,0)=2.70864%|dock_size(4,1,0)=2.48902%|dock_size(2,0,0)=30.6003%|dock_size(3,0,1)=8.27233%|" 3 | textFiles: 0 "controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c" 4 | -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/body_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/body_3dx.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/caster_wheel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/caster_wheel.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/parquet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/parquet.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/serial_plug.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/serial_plug.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/smooth_rubber.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/smooth_rubber.png -------------------------------------------------------------------------------- /pioneer3dx bug1/worlds/textures/top_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug1/worlds/textures/top_3dx.png -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/.obstacle_avoidance_with_kinect.exe.signature: -------------------------------------------------------------------------------- 1 | 15532e9c-c2e75a40-19fb921e-94b080e5-81eb4c0a 2 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c: -------------------------------------------------------------------------------- 1 | /* 2 | * File: obstacle_avoidance_with_kinect.c 3 | * Date: August 24th, 2011 4 | * Description: A Braitenberg-like controller moving a Pioneer 3-DX equipped with a kinect. 5 | * Author: can 6 | * 7 | * Copyright (c) 2011 Cyberbotics - www.cyberbotics.com 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define MAX_SPEED 10.0 21 | #define CRUISING_SPEED 7.5 22 | #define TOLERANCE -0.1 23 | #define OBSTACLE_THRESHOLD 0.5 24 | #define SLOWDOWN_FACTOR 0.5 25 | 26 | #define TURN_LEFT_FOLLOW 1 27 | #define TURN_RIGHT_FOLLOW 2 28 | #define GO_TO_TARGET 3 29 | 30 | static WbDeviceTag backGPS, centerGPS; 31 | static WbDeviceTag leftWheel, rightWheel; 32 | static WbDeviceTag kinect; 33 | const float* kinectValues; 34 | static int time_step = 64; 35 | const double *point1; 36 | const double *point2; 37 | static WbDeviceTag distance_sensor[8]; 38 | double distance_value[8]; 39 | int main() 40 | { 41 | wb_robot_init(); 42 | time_step = wb_robot_get_basic_time_step(); 43 | leftWheel = wb_robot_get_device("leftWheel"); 44 | rightWheel = wb_robot_get_device("rightWheel"); 45 | kinect = wb_robot_get_device("kinect"); 46 | wb_camera_enable(kinect, time_step); 47 | int kinectWidth = wb_camera_get_width(kinect); 48 | int kinectHeight = wb_camera_get_height(kinect); 49 | int halfWidth = kinectWidth/2; 50 | int viewHeight = kinectHeight/2 + 10; 51 | double maxRange = wb_camera_get_max_range(kinect); 52 | double rangeThreshold = 0.7; 53 | double invMaxRangeTimesWidth = 1.0 / (maxRange * kinectWidth); 54 | //target coordinates 55 | static float targetX=0; 56 | static float targetZ=4.684; 57 | //initial position angle of the pioneer 58 | float initialAngle; 59 | 60 | backGPS = wb_robot_get_device("frontGps"); //name is wrong, it is actually at back 61 | centerGPS = wb_robot_get_device("centerGps"); 62 | wb_gps_enable(backGPS,time_step); 63 | wb_gps_enable(centerGPS,time_step); 64 | // set servos' positions 65 | wb_servo_set_position(leftWheel,INFINITY); 66 | wb_servo_set_position(rightWheel,INFINITY); 67 | // set speeds 68 | wb_servo_set_velocity(leftWheel, 0.0); 69 | wb_servo_set_velocity(rightWheel, 0.0); 70 | //perform one control loop 71 | wb_robot_step(time_step); 72 | distance_sensor[0] = wb_robot_get_device("so0"); 73 | distance_sensor[1] = wb_robot_get_device("so1"); 74 | distance_sensor[2] = wb_robot_get_device("so2"); 75 | distance_sensor[3] = wb_robot_get_device("so3"); 76 | distance_sensor[4] = wb_robot_get_device("so4"); 77 | distance_sensor[5] = wb_robot_get_device("so5"); 78 | distance_sensor[6] = wb_robot_get_device("so6"); 79 | distance_sensor[7] = wb_robot_get_device("so7"); 80 | wb_distance_sensor_enable(distance_sensor[0],time_step); 81 | wb_distance_sensor_enable(distance_sensor[1],time_step); 82 | wb_distance_sensor_enable(distance_sensor[2],time_step); 83 | wb_distance_sensor_enable(distance_sensor[3],time_step); 84 | wb_distance_sensor_enable(distance_sensor[4],time_step); 85 | wb_distance_sensor_enable(distance_sensor[5],time_step); 86 | wb_distance_sensor_enable(distance_sensor[6],time_step); 87 | wb_distance_sensor_enable(distance_sensor[7],time_step); 88 | 89 | // init dynamic variables 90 | double leftObstacle = 0.0, rightObstacle = 0.0, obstacle = 0.0; 91 | double deltaObstacle = 0.0; 92 | double leftSpeed = CRUISING_SPEED, rightSpeed = CRUISING_SPEED; 93 | double speedFactor = 1.0; 94 | float targetAngle; 95 | float currentAngle; 96 | float value = 0.0; 97 | int i = 0; 98 | double sideObstacle = 0.0; 99 | int mode=0; 100 | 101 | //setting the initial position angle to target 102 | point2 = (float*) wb_gps_get_values(centerGPS); 103 | initialAngle = atan2(targetZ-point2[2],targetX-point2[0]); 104 | 105 | while (1) { 106 | 107 | //detector sampling... 108 | for(i=0; i<8; i++){ 109 | distance_value[i] = wb_distance_sensor_get_value(distance_sensor[i]); 110 | } 111 | /* 112 | for(i=0; i<8; i++){ 113 | printf("%d-%f ",i,distance_value[i]); 114 | } 115 | printf("\n"); 116 | */ 117 | sideObstacle = distance_value[0] + distance_value[1] + distance_value[6] + distance_value[7]; 118 | 119 | kinectValues = (float*) wb_camera_get_range_image(kinect); 120 | for (i = 0; i < halfWidth; i++) { 121 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, i, viewHeight); 122 | if(value < rangeThreshold) { 123 | leftObstacle += value; 124 | } 125 | value = wb_camera_range_image_get_depth(kinectValues, kinectWidth, kinectWidth - i, viewHeight); 126 | if(value < rangeThreshold) { 127 | rightObstacle += value; 128 | } 129 | } 130 | obstacle = leftObstacle + rightObstacle; 131 | point1 = (float*) wb_gps_get_values(backGPS); 132 | point2 = (float*) wb_gps_get_values(centerGPS); 133 | targetAngle = atan2(targetZ-point1[2],targetX-point1[0]); 134 | currentAngle = atan2(point2[2]-point1[2],point2[0]-point1[0]); 135 | 136 | //if reach the target, stop 137 | if(sqrt(pow(targetZ-point2[2],2) + pow(targetX-point2[0],2))<0.6) 138 | { 139 | wb_servo_set_velocity(leftWheel, 0); 140 | wb_servo_set_velocity(rightWheel, 0); 141 | printf("\n\n\n!!! Target reached !!!\n"); 142 | break; 143 | } 144 | 145 | //mode change cases 146 | if(obstacle > 5.0) 147 | { 148 | deltaObstacle = - (leftObstacle - rightObstacle) * invMaxRangeTimesWidth; 149 | if(deltaObstacle > TOLERANCE) 150 | mode = TURN_RIGHT_FOLLOW; 151 | else 152 | mode = TURN_LEFT_FOLLOW; 153 | }else if(obstacle<1.0 && fabs(initialAngle-targetAngle)<0.2) 154 | mode = GO_TO_TARGET; 155 | 156 | printf("inital:%2f , taget:%2f",initialAngle,targetAngle); 157 | switch(mode){ 158 | case TURN_LEFT_FOLLOW: 159 | printf("TURN_LEFT_FOLLOW \n"); 160 | if(obstacle > 3.0) 161 | { 162 | obstacle = 1.0 - obstacle * invMaxRangeTimesWidth;// compute the relevant overall quantity of obstacle 163 | speedFactor = (obstacle > OBSTACLE_THRESHOLD) ? 0.0 : SLOWDOWN_FACTOR; 164 | leftSpeed = speedFactor * CRUISING_SPEED; 165 | rightSpeed = CRUISING_SPEED; 166 | }else if(distance_value[0]<1.0){ 167 | leftSpeed +=1; 168 | rightSpeed -=1; 169 | }else{ 170 | leftSpeed = CRUISING_SPEED; 171 | rightSpeed = CRUISING_SPEED; 172 | } 173 | break; 174 | case TURN_RIGHT_FOLLOW: 175 | printf("TURN_RIGHT_FOLLOW \n"); 176 | if(obstacle > 3.0) 177 | { 178 | obstacle = 1.0 - obstacle * invMaxRangeTimesWidth;// compute the relevant overall quantity of obstacle 179 | speedFactor = (obstacle > OBSTACLE_THRESHOLD) ? 0.0 : SLOWDOWN_FACTOR; 180 | leftSpeed = CRUISING_SPEED; 181 | rightSpeed = speedFactor * CRUISING_SPEED; 182 | }else if(distance_value[0]<1.0){ 183 | leftSpeed -=1; 184 | rightSpeed +=1; 185 | }else{ 186 | leftSpeed = CRUISING_SPEED; 187 | rightSpeed = CRUISING_SPEED; 188 | } 189 | break; 190 | case GO_TO_TARGET: 191 | printf("GO_TO_TARGET \n"); 192 | if(fabs(targetAngle-currentAngle)>0.3){ 193 | if(targetAngle < currentAngle){ 194 | leftSpeed -=0.5; 195 | rightSpeed +=0.5; 196 | } 197 | else { 198 | leftSpeed +=0.5; 199 | rightSpeed -=0.5; 200 | } 201 | }else { 202 | printf("CRUISING_SPEED \n"); 203 | leftSpeed = CRUISING_SPEED; 204 | rightSpeed = CRUISING_SPEED; 205 | } 206 | break; 207 | } 208 | // set speeds 209 | wb_servo_set_velocity(leftWheel, leftSpeed); 210 | wb_servo_set_velocity(rightWheel, rightSpeed); 211 | printf("%2f, %2f \n",leftObstacle,rightObstacle); 212 | // run simulation 213 | wb_robot_step(time_step); 214 | leftObstacle = 0.0; 215 | rightObstacle = 0.0; 216 | } 217 | } 218 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.d: -------------------------------------------------------------------------------- 1 | obstacle_avoidance_with_kinect.o: obstacle_avoidance_with_kinect.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/servo.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/camera.h \ 6 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/distance_sensor.h \ 7 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/gps.h 8 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.o -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | ### Standard Makefile for Webots controllers 3 | ### 4 | ### Platform: Windows, Mac OS X, Linux 5 | ### Language: C, C++ and Java 6 | ### 7 | ### Authors: Olivier Michel - www.cyberbotics.com 8 | ### Revised: Yvan Bourquin - September 30th, 2009. 9 | ### 10 | ### Uncomment the variables to customize the Makefile 11 | 12 | ### -----C Sources----- 13 | ### 14 | ### if your controller uses several C sources files: 15 | # C_SOURCES = my_controller.c my_clever_algo.c my_gui.c 16 | 17 | ### -----C++ Sources----- 18 | ### 19 | ### if your controller uses several C++ sources files: 20 | # CPP_SOURCES = my_controller.cpp my_clever_algo.cpp my_gui.cpp 21 | ### or 22 | # CC_SOURCES = my_controller.cc my_clever_algo.cc my_gui.cc 23 | 24 | ### -----C/C++ Options----- 25 | ### 26 | ### if special CFLAGS are necessary, for example to set optimization level or 27 | ### to find include files: 28 | # CFLAGS=-O3 -I/my_library_path/include 29 | ### 30 | ### if your controller needs additional libraries: 31 | # LIBRARIES=-L/path/to/my/library -lmy_library -lmy_other_library 32 | ### 33 | ### if you want to use the C API in your C++ program add: 34 | # USE_C_API=1 35 | 36 | ### -----Java Options----- 37 | ### 38 | ### if your Java controller needs additional libraries, you should define 39 | ### the CLASSPATH environment variable as explained in the Webots User Guide 40 | # CLASSPATH=relative/mylib.jar 41 | 42 | ### Do not modify: this includes Webots global Makefile.include 43 | space := 44 | space += 45 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 46 | include $(WEBOTS_HOME_PATH)/resources/projects/default/controllers/Makefile.include 47 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/supervisor.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define XCOORD 0 6 | #define ZCOORD 2 7 | 8 | WbNodeRef robot; 9 | WbFieldRef robot_translation_field; 10 | const double *robot_translation; 11 | static int time_step = 64; 12 | 13 | 14 | int main() { 15 | wb_robot_init(); 16 | //time_step = wb_robot_get_basic_time_step(); 17 | robot = wb_supervisor_node_get_from_def("PIONEER_3DX"); 18 | robot_translation_field = wb_supervisor_node_get_field(robot,"translation"); 19 | wb_robot_step(time_step); 20 | while(1){ 21 | robot_translation = wb_supervisor_field_get_sf_vec3f(robot_translation_field); 22 | wb_robot_step(time_step); 23 | printf("Pioneer 3DX is at: (%f, %f)\n",robot_translation[XCOORD],robot_translation[ZCOORD]); 24 | wb_robot_step(time_step); 25 | } 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/supervisor.d: -------------------------------------------------------------------------------- 1 | supervisor.o: supervisor.c \ 2 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/robot.h \ 3 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/types.h \ 4 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/supervisor.h \ 5 | C:\Program\ Files\ (x86)\Webots/include/controller/c/webots/nodes.h 6 | -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/supervisor.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/supervisor/supervisor.exe -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/supervisor.exe_webots: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/supervisor/supervisor.exe_webots -------------------------------------------------------------------------------- /pioneer3dx bug2/controllers/supervisor/supervisor.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/controllers/supervisor/supervisor.o -------------------------------------------------------------------------------- /pioneer3dx bug2/protos/Pioneer_DistanceSensor.proto: -------------------------------------------------------------------------------- 1 | PROTO Pioneer_DistanceSensor [ 2 | field SFVec3f translation 0 0.114 0 3 | field SFRotation rotation 0 1 0 0 4 | field SFString name "so" 5 | field SFString type "infra-red" 6 | ] 7 | { 8 | DistanceSensor { 9 | translation IS translation 10 | rotation IS rotation 11 | name IS name 12 | type IS type 13 | children [ 14 | DEF BLACK_BODY_SHAPE Shape { 15 | appearance Appearance { 16 | material Material { 17 | diffuseColor 0 0 0 18 | shininess 0.0976563 19 | specularColor 0.401201 0.401201 0.401201 20 | } 21 | } 22 | geometry IndexedFaceSet { 23 | coord Coordinate { 24 | point [ 25 | 0.0027176 -0.0082813 -0.0199851 26 | 0.0027176 -0.0152989 -0.0152947 27 | 0.0027176 -0.0199874 -0.0082758 28 | 0.0027176 -0.021633 3e-06 29 | 0.0027176 -0.0199851 0.0082813 30 | 0.0027176 -0.0152947 0.0152989 31 | 0.0027176 -0.0082758 0.0199874 32 | 0.0027176 3e-06 0.021633 33 | 0.0027176 0.0082813 0.0199851 34 | 0.0027176 0.0152989 0.0152947 35 | 0.0027176 0.0199874 0.0082758 36 | 0.0027176 0.021633 -3e-06 37 | 0.0027176 0.0199851 -0.0082813 38 | 0.0027176 0.0152947 -0.0152989 39 | 0.0027176 0.0082758 -0.0199874 40 | 0.0027176 -3e-06 -0.021633 41 | 3.68e-05 -0.0082813 -0.0199851 42 | 3.68e-05 -0.0152989 -0.0152947 43 | 3.68e-05 -0.0199874 -0.0082758 44 | 3.68e-05 -0.021633 3e-06 45 | 3.68e-05 -0.0199851 0.0082813 46 | 3.68e-05 -0.0152947 0.0152989 47 | 3.68e-05 -0.0082758 0.0199874 48 | 3.68e-05 3e-06 0.021633 49 | 3.68e-05 0.0082813 0.0199851 50 | 3.68e-05 0.0152989 0.0152947 51 | 3.68e-05 0.0199874 0.0082758 52 | 3.68e-05 0.021633 -3e-06 53 | 3.68e-05 0.0199851 -0.0082813 54 | 3.68e-05 0.0152947 -0.0152989 55 | 3.68e-05 0.0082758 -0.0199874 56 | 3.68e-05 -3e-06 -0.021633 57 | 0.0027176 -0.0071569 -0.0172717 58 | 0.0027176 -0.0132217 -0.0132181 59 | 0.0027176 -0.0172736 -0.0071522 60 | 0.0027176 -0.0186958 2.6e-06 61 | 0.0027176 -0.0172717 0.0071569 62 | 0.0027176 -0.0132181 0.0132217 63 | 0.0027176 -0.0071522 0.0172736 64 | 0.0027176 2.6e-06 0.0186958 65 | 0.0027176 0.0071569 0.0172717 66 | 0.0027176 0.0132217 0.0132181 67 | 0.0027176 0.0172736 0.0071522 68 | 0.0027176 0.0186958 -2.6e-06 69 | 0.0027176 0.0172717 -0.0071569 70 | 0.0027176 0.0132181 -0.0132217 71 | 0.0027176 0.0071522 -0.0172736 72 | 0.0027176 -2.6e-06 -0.0186958 73 | 0.0004008 -0.0071569 -0.0172717 74 | 0.0004008 -0.0132217 -0.0132181 75 | 0.0004008 -0.0172736 -0.0071522 76 | 0.0004008 -0.0186958 2.6e-06 77 | 0.0004008 -0.0172717 0.0071569 78 | 0.0004008 -0.0132181 0.0132217 79 | 0.0004008 -0.0071522 0.0172736 80 | 0.0004008 2.6e-06 0.0186958 81 | 0.0004008 0.0071569 0.0172717 82 | 0.0004008 0.0132217 0.0132181 83 | 0.0004008 0.0172736 0.0071522 84 | 0.0004008 0.0186958 -2.6e-06 85 | 0.0004008 0.0172717 -0.0071569 86 | 0.0004008 0.0132181 -0.0132217 87 | 0.0004008 0.0071522 -0.0172736 88 | 0.0004008 -2.6e-06 -0.0186958 89 | ] 90 | } 91 | coordIndex [ 92 | 63, 46, 62, -1, 63, 47, 46, -1, 61, 62, 93 | 46, -1, 61, 46, 45, -1, 60, 61, 45, -1, 94 | 60, 45, 44, -1, 60, 44, 43, -1, 60, 43, 95 | 59, -1, 59, 43, 42, -1, 59, 42, 58, -1, 96 | 57, 58, 42, -1, 57, 42, 41, -1, 56, 57, 97 | 41, -1, 56, 41, 40, -1, 56, 40, 39, -1, 98 | 56, 39, 55, -1, 55, 39, 38, -1, 55, 38, 99 | 54, -1, 53, 54, 38, -1, 53, 38, 37, -1, 100 | 52, 53, 37, -1, 52, 37, 36, -1, 52, 36, 101 | 35, -1, 52, 35, 51, -1, 51, 35, 34, -1, 102 | 51, 34, 50, -1, 49, 50, 34, -1, 49, 34, 103 | 33, -1, 49, 33, 32, -1, 49, 32, 48, -1, 104 | 15, 14, 46, -1, 15, 46, 47, -1, 13, 45, 105 | 46, -1, 13, 46, 14, -1, 12, 44, 45, -1, 106 | 12, 45, 13, -1, 12, 11, 43, -1, 12, 43, 107 | 44, -1, 11, 10, 42, -1, 11, 42, 43, -1, 108 | 9, 41, 42, -1, 9, 42, 10, -1, 8, 40, 109 | 41, -1, 8, 41, 9, -1, 8, 7, 39, -1, 110 | 8, 39, 40, -1, 7, 6, 38, -1, 7, 38, 111 | 39, -1, 5, 37, 38, -1, 5, 38, 6, -1, 112 | 4, 36, 37, -1, 4, 37, 5, -1, 4, 3, 113 | 35, -1, 4, 35, 36, -1, 3, 2, 34, -1, 114 | 3, 34, 35, -1, 1, 33, 34, -1, 1, 34, 115 | 2, -1, 1, 0, 32, -1, 1, 32, 33, -1, 116 | 15, 47, 32, -1, 32, 0, 15, -1, 14, 15, 117 | 31, -1, 14, 31, 30, -1, 14, 30, 29, -1, 118 | 14, 29, 13, -1, 13, 29, 28, -1, 13, 28, 119 | 12, -1, 11, 12, 28, -1, 11, 28, 27, -1, 120 | 10, 11, 27, -1, 10, 27, 26, -1, 10, 26, 121 | 25, -1, 10, 25, 9, -1, 9, 25, 24, -1, 122 | 9, 24, 8, -1, 7, 8, 24, -1, 7, 24, 123 | 23, -1, 6, 7, 23, -1, 6, 23, 22, -1, 124 | 6, 22, 21, -1, 6, 21, 5, -1, 5, 21, 125 | 20, -1, 5, 20, 4, -1, 3, 4, 20, -1, 126 | 3, 20, 19, -1, 2, 3, 19, -1, 2, 19, 127 | 18, -1, 2, 18, 17, -1, 2, 17, 1, -1, 128 | 0, 1, 17, -1, 0, 17, 16, -1, 0, 16, 129 | 31, -1, 0, 31, 15, -1, 47, 48, 32, -1, 130 | 47, 63, 48, -1 131 | ] 132 | creaseAngle 1 133 | } 134 | } 135 | DEF BOTTOM_DISC_SHAPE Shape { 136 | appearance Appearance { 137 | material Material { 138 | shininess 0.0976563 139 | specularColor 0.401201 0.401201 0.401201 140 | } 141 | texture ImageTexture { 142 | url [ 143 | "textures/honeycomb.png" 144 | ] 145 | } 146 | } 147 | geometry IndexedFaceSet { 148 | coord DEF coord_BOTTOM_DISC Coordinate { 149 | point [ 150 | 0.0001602 3.59e-05 0.0196526 151 | 0.0001602 -0.0075108 0.0181533 152 | 0.0001602 -0.0139092 0.0138801 153 | 0.0001602 -0.0181854 0.0074836 154 | 0.0001602 0.0075819 0.0181497 155 | 0.0001602 0.0139783 0.0138736 156 | 0.0001602 0.0182515 0.0074751 157 | 0.0001602 0.0197508 -7.15e-05 158 | 0.0001602 0.018248 -0.0076175 159 | 0.0001602 0.0139718 -0.014014 160 | 0.0001602 0.0075734 -0.0182871 161 | 0.0001602 2.67e-05 -0.0197864 162 | 0.0001602 -0.0075193 -0.0182836 163 | 0.0001602 -0.0139157 -0.0140075 164 | 0.0001602 -0.0181889 -0.007609 165 | 0.0001602 -0.0196882 -6.23e-05 166 | ] 167 | } 168 | texCoord TextureCoordinate { 169 | point [ 170 | 0.404999 0.9776 171 | 0.229461 0.90489 172 | 0.09511 0.770539 173 | 0.404999 0.9776 174 | 0.09511 0.770539 175 | 0.0223996 0.595001 176 | 0.595 0.9776 177 | 0.404999 0.9776 178 | 0.0223996 0.595001 179 | 0.595 0.9776 180 | 0.0223996 0.595001 181 | 0.0223995 0.405 182 | 0.770539 0.90489 183 | 0.595 0.9776 184 | 0.0223995 0.405 185 | 0.0223995 0.405 186 | 0.0951098 0.229461 187 | 0.770539 0.90489 188 | 0.770539 0.90489 189 | 0.0951098 0.229461 190 | 0.90489 0.770539 191 | 0.90489 0.770539 192 | 0.0951098 0.229461 193 | 0.229461 0.09511 194 | 0.9776 0.595001 195 | 0.90489 0.770539 196 | 0.229461 0.09511 197 | 0.9776 0.595001 198 | 0.229461 0.09511 199 | 0.405 0.0223995 200 | 0.9776 0.405 201 | 0.9776 0.595001 202 | 0.405 0.0223995 203 | 0.9776 0.405 204 | 0.405 0.0223995 205 | 0.595001 0.0223997 206 | 0.90489 0.229462 207 | 0.9776 0.405 208 | 0.595001 0.0223997 209 | 0.90489 0.229462 210 | 0.595001 0.0223997 211 | 0.770539 0.0951104 212 | ] 213 | } 214 | coordIndex [ 215 | 8, 7, 6, -1, 8, 6, 5, -1, 9, 8, 216 | 5, -1, 9, 5, 4, -1, 10, 9, 4, -1, 217 | 4, 0, 10, -1, 10, 0, 11, -1, 11, 0, 218 | 1, -1, 12, 11, 1, -1, 12, 1, 2, -1, 219 | 13, 12, 2, -1, 13, 2, 3, -1, 14, 13, 220 | 3, -1, 14, 3, 15, -1 221 | ] 222 | texCoordIndex [ 223 | 0, 1, 2, -1, 3, 4, 5, -1, 6, 7, 224 | 8, -1, 9, 10, 11, -1, 12, 13, 14, -1, 225 | 15, 16, 17, -1, 18, 19, 20, -1, 21, 22, 226 | 23, -1, 24, 25, 26, -1, 27, 28, 29, -1, 227 | 30, 31, 32, -1, 33, 34, 35, -1, 36, 37, 228 | 38, -1, 39, 40, 41, -1 229 | ] 230 | creaseAngle 1 231 | } 232 | } 233 | DEF SONAR_GLASS_SHAPE Shape { 234 | appearance Appearance { 235 | material Material { 236 | diffuseColor 1 0.995646 1 237 | shininess 0.0976563 238 | specularColor 0.401201 0.401201 0.401201 239 | transparency 0.699218 240 | } 241 | } 242 | geometry IndexedFaceSet { 243 | coord Coordinate { 244 | point [ 245 | 0.0022441 -0.0078844 -0.017925 246 | 0.0022441 -0.014335 -0.0136148 247 | 0.0022441 -0.0186452 -0.0071641 248 | 0.0022441 -0.0201588 0.0004449 249 | 0.0022441 -0.0186452 0.008054 250 | 0.0022441 -0.014335 0.0145046 251 | 0.0022441 -0.0078844 0.0188148 252 | 0.0022441 -0.0002753 0.0203284 253 | 0.0022441 0.0073338 0.0188148 254 | 0.0022441 0.0137844 0.0145046 255 | 0.0022441 0.0180946 0.008054 256 | 0.0022441 0.0196081 0.0004449 257 | 0.0022441 0.0180946 -0.0071642 258 | 0.0022441 0.0137844 -0.0136148 259 | 0.0022441 0.0073337 -0.017925 260 | 0.0022441 -0.0002753 -0.0194385 261 | ] 262 | } 263 | coordIndex [ 264 | 12, 11, 10, -1, 12, 10, 9, -1, 13, 12, 265 | 9, -1, 13, 9, 8, -1, 14, 13, 8, -1, 266 | 14, 8, 7, -1, 15, 14, 7, -1, 15, 7, 267 | 6, -1, 0, 15, 6, -1, 0, 6, 5, -1, 268 | 1, 0, 5, -1, 1, 5, 4, -1, 2, 1, 269 | 4, -1, 2, 4, 3, -1 270 | ] 271 | creaseAngle 1 272 | } 273 | } 274 | ] 275 | lookupTable [ 276 | 0 1024 0 277 | 0.2 1024 0.1 278 | 0.4 0 0.1 279 | ] 280 | } 281 | } 282 | -------------------------------------------------------------------------------- /pioneer3dx bug2/protos/textures/honeycomb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/protos/textures/honeycomb.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/.pioneer3dx_with_kinect.wbt.project: -------------------------------------------------------------------------------- 1 | Webots Project File version 6.0 2 | perspectives: 0 "Default" "layout2|name=3D view;caption=3D view;state=4212732;dir=5;layer=0;row=0;pos=0;prop=100000;bestw=0.951684%;besth=2.08024%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=text editor;caption=Text editor;state=6293500;dir=2;layer=0;row=0;pos=0;prop=100000;bestw=35.2123%;besth=62.8529%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|name=console;caption=Console;state=6293500;dir=3;layer=0;row=1;pos=0;prop=100000;bestw=89.7511%;besth=20.6538%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=351;floaty=531;floatw=400;floath=492|name=scene tree;caption=Scene tree;state=6293500;dir=4;layer=1;row=0;pos=0;prop=100000;bestw=17.716%;besth=83.9525%;minw=32;minh=32;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1|dock_size(5,0,0)=2.70864%|dock_size(4,1,0)=2.48902%|dock_size(2,0,0)=23.9385%|dock_size(3,0,1)=9.44363%|" 3 | textFiles: 0 "controllers/obstacle_avoidance_with_kinect/obstacle_avoidance_with_kinect.c" 4 | -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/body_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/body_3dx.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/caster_wheel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/caster_wheel.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/parquet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/parquet.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/serial_plug.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/serial_plug.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/smooth_rubber.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/smooth_rubber.png -------------------------------------------------------------------------------- /pioneer3dx bug2/worlds/textures/top_3dx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aksakalli/BugAlgorithms/c8d7e44a5792c85818de1c6f329aa9d72de049c9/pioneer3dx bug2/worlds/textures/top_3dx.png --------------------------------------------------------------------------------