├── .gitattributes ├── .gitignore ├── 3DPrintable_parts ├── OPENSCAD │ ├── AHR_PS3Camera_holder.scad │ ├── AHR_Pulley_1.scad │ ├── AHR_Pulley_2.scad │ ├── AHR_x_carriage.scad │ ├── AHR_x_carriage_belt.scad │ ├── AHR_x_carriage_pusher.scad │ ├── AHR_x_motor_mount.scad │ ├── AHR_y_axis_v2.scad │ ├── AHR_y_carriage.scad │ ├── PrintableBearing.scad │ ├── puck_v2.scad │ └── pusher.scad └── STL │ ├── AHR_PS3Camera_holder.stl │ ├── AHR_Pulley_1.stl │ ├── AHR_Pulley_2.stl │ ├── AHR_x_carriage.stl │ ├── AHR_x_carriage_belt.stl │ ├── AHR_x_carriage_pusher.stl │ ├── AHR_x_motor_mount.stl │ ├── AHR_y_axis_v2.stl │ ├── AHR_y_carriage.stl │ ├── PrintableBearing.stl │ ├── puck_v2.stl │ └── pusher.stl ├── Arduino ├── AHRobot │ ├── AHRobot.ino │ ├── Camera.ino │ ├── Configuration.h │ ├── Definitions.h │ ├── Robot.ino │ └── Steppers.ino └── Utils │ ├── AHR_Motor_Test │ ├── AHR_Motor_Test.ino │ ├── Configuration.h │ ├── Definitions.h │ └── Steppers.ino │ ├── AHR_Motor_Test_X │ ├── AHR_Motor_Test_X.ino │ ├── Configuration.h │ ├── Definitions.h │ └── Steppers.ino │ ├── AHR_Motor_Test_Y │ ├── AHR_Motor_Test_Y.ino │ ├── Configuration.h │ ├── Definitions.h │ └── Steppers.ino │ └── AHR_paint │ ├── AHR_paint.ino │ ├── Configuration.h │ ├── Definitions.h │ ├── GCODE.h │ └── Steppers.ino ├── LICENSE ├── README.md └── VisionSystem ├── PS3Camera ├── Binaries │ ├── AHR.bat │ ├── AHR.exe │ ├── CHECK_HSV.exe │ ├── log.txt │ ├── opencv_core247.dll │ ├── opencv_ffmpeg247.dll │ ├── opencv_highgui247.dll │ ├── opencv_imgproc247.dll │ └── opencv_video247.dll ├── CHECK_HSV.cpp └── Source │ ├── AHR.sln │ └── AHR │ └── AHR_2.cpp └── readme.txt /.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 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_PS3Camera_holder.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // PS3 Camera holder (needs a 8mm carbon tube) 6 | 7 | module rod_base(angle=20,height=20) 8 | { 9 | rotate([angle,0,0]) cylinder(r=6,h=height,$fn=32); 10 | } 11 | 12 | module rod_holes(angle=20,height=22) 13 | { 14 | rotate([angle,0,0]) cylinder(r=4.2,h=height,$fn=32); 15 | } 16 | 17 | module PS3Cam_base() 18 | { 19 | cylinder(r=12,h=4.5,$fn=64); 20 | translate([-6,-29,0]) cube([12,20,4.5]); 21 | // Support to print the rod holder 22 | translate([-3,-38.3,0]) cube([6,30,4.5]); 23 | // Rod holder 24 | translate([0,-18,2]) rod_base(72); 25 | } 26 | 27 | module PS3Cam_holes() 28 | { 29 | translate([-15,3.4,-1]) cube([30,20,10]); 30 | translate([0,0,-1]) cylinder(r=9,h=6,$fn=64); 31 | // Rod holder 32 | translate([0,-18,2]) rod_holes(72); 33 | // Cut elements lower than 0 34 | translate([0,0,-5]) cube([200,200,10],center=true); 35 | } 36 | 37 | module base() 38 | { 39 | translate([-20,0,0]) cube([40,11,3]); 40 | translate([-8,-20,0]) cube([16,42,3]); 41 | //translate([-8,-33,0]) cube([16,49,3]); 42 | //translate([-20,-11-22,0]) cube([40,11,3]); 43 | // Rod reinforcement 44 | translate([-5,0,2.5]) cube([10,22,11]); 45 | 46 | // Rod holder 47 | translate([0,24,10]) rod_base(68,24); 48 | } 49 | 50 | module holes() 51 | { 52 | translate([-15,5.5,0]) cylinder(r=1.7,h=10,$fn=8,center=true); 53 | translate([15,5.5,0]) cylinder(r=1.7,h=10,$fn=8,center=true); 54 | translate([0,-11,0]) cylinder(r=1.7,h=10,$fn=8,center=true); 55 | 56 | //translate([-15,-27.5,0]) cylinder(r=1.7,h=10,$fn=8,center=true); 57 | //translate([15,-27.5,0]) cylinder(r=1.7,h=10,$fn=8,center=true); 58 | 59 | // rod holder 60 | translate([0,24,10]) rod_holes(68,22); 61 | 62 | // Cut elements lower than 0 63 | translate([0,0,-5]) cube([200,200,10],center=true); 64 | } 65 | 66 | 67 | difference() 68 | { 69 | base(); 70 | holes(); 71 | } 72 | 73 | 74 | translate([36,10,0]) difference() 75 | { 76 | PS3Cam_base(); 77 | PS3Cam_holes(); 78 | } 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_x_carriage.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // X axis carriage 6 | // designed for only two bearings (I could test a 3 bearings version) 7 | 8 | diameter = 80; 9 | rod_separation=45; 10 | bearing_diameter=15.5; 11 | bearing_length=26; 12 | 13 | module horizontal_bearing_holes(bearings=1){ 14 | cutter_lenght = 4+bearings*25; 15 | one_holder_lenght = 8+25; 16 | holder_lenght = 8+bearings*25; 17 | 18 | // Main bearing cut 19 | difference(){ 20 | translate(v=[0,0,12]) rotate(a=[90,0,0]) translate(v=[0,0,-cutter_lenght/2]) cylinder(h = cutter_lenght, r=bearing_diameter/2, $fn=64); 21 | // Bearing retainers 22 | translate(v=[0,-holder_lenght/2,3]) cube(size = [24,6,34], center = true); 23 | translate(v=[0,+holder_lenght/2,3]) cube(size = [24,6,34], center = true); 24 | } 25 | 26 | // Ziptie cutouts 27 | 28 | ziptie_cut_ofset = 0; 29 | for ( i = [0 : bearings-1] ){ 30 | // For easier positioning I move them by half of one 31 | // bearing holder then add each bearign lenght and then center again 32 | translate(v=[0,-holder_lenght/2,0]) translate(v=[0,one_holder_lenght/2+i*25,0]) difference(){ 33 | union(){ 34 | translate(v=[0,2-6,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 4, r=12.5, $fn=50); 35 | translate(v=[0,2+6,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 4, r=12.5, $fn=50); 36 | } 37 | translate(v=[0,10,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 24, r=9.5, $fn=50); 38 | } 39 | } 40 | 41 | 42 | } 43 | 44 | module border() 45 | { 46 | difference() 47 | { 48 | cylinder (r=diameter/2, h=4.5,center=true,$fn=80); 49 | cylinder (r=diameter/2-4.5, h=6,center=true,$fn=80); 50 | } 51 | } 52 | 53 | module x_carriage_beltcut(){ 54 | // Cut in the middle for belt 55 | translate([-5.5+1,16.5,8]) cube([8,25,15]); 56 | // Cut clearing space for the belt 57 | //translate([-38,5,8]) cube([40,13,15]); 58 | // Belt slit 59 | translate([-32,16,8]) cube([80,1.25,15]); 60 | // Smooth entrance 61 | translate([-30,16.5,15.3]) rotate([45,0,0]) cube([80,15,15]); 62 | // Smooth the block on the upper side 63 | //translate([-66,27,15]) rotate([30,0,0]) cube([80,15,15]); 64 | // Teeth cuts (GT2) 65 | for ( i = [0 : 32] ){ 66 | translate([30-i*2,14.5,8]) cube([1.15,2,15]); 67 | } 68 | } 69 | 70 | module belt() 71 | { 72 | difference() 73 | { 74 | translate ([9,0,8]) cube([12,38,16],center=true); 75 | translate([25,0,-2]) rotate([0,0,90]) x_carriage_beltcut(); 76 | } 77 | } 78 | 79 | module x_carriage_base() 80 | { 81 | cylinder (r=diameter/2, h=2,center=true,$fn=80); 82 | translate([0,0,2]) border(); 83 | 84 | // bearing bases 85 | translate([rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 86 | translate([-rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 87 | 88 | //belt(); 89 | } 90 | 91 | module x_carriage_holes() 92 | { 93 | // bearing holes 94 | translate([rod_separation/2,0,-2]) horizontal_bearing_holes(); 95 | translate([-rod_separation/2,0,-2]) horizontal_bearing_holes(); 96 | 97 | // holes for belt 98 | translate([7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 99 | translate([7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 100 | translate([-7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 101 | translate([-7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 102 | 103 | 104 | // M3 holes 105 | translate([0,-30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 106 | translate([15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 107 | translate([-15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 108 | translate([0,30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 109 | translate([15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 110 | translate([-15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 111 | 112 | translate([rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 113 | translate([rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 114 | translate([-rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 115 | translate([-rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 116 | } 117 | 118 | difference() 119 | { 120 | x_carriage_base(); 121 | x_carriage_holes(); 122 | } 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_x_carriage_belt.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // X axis carriage belt holder 6 | 7 | diameter = 80; 8 | rod_separation=45; 9 | bearing_diameter=15.5; 10 | bearing_length=26; 11 | 12 | 13 | module border() 14 | { 15 | difference() 16 | { 17 | cylinder (r=diameter/2, h=4.5,center=true,$fn=80); 18 | cylinder (r=diameter/2-4.5, h=6,center=true,$fn=80); 19 | } 20 | } 21 | 22 | module x_carriage_beltcut(){ 23 | // Cut in the middle for belt 24 | translate([0,0,12]) cube([8,25,15],center=true); 25 | // Cut clearing space for the belt 26 | //translate([-38,5,8]) cube([40,13,15]); 27 | // Belt slit 28 | translate([0,0,12]) cube([80,1.25,15],center=true); 29 | // Smooth entrance 30 | translate([0,0,24]) rotate([45,0,0]) cube([80,15,15],center=true); 31 | // Smooth the block on the upper side 32 | //translate([-66,27,15]) rotate([30,0,0]) cube([80,15,15]); 33 | // Teeth cuts (GT2) 34 | for ( i = [-16 : 16] ){ 35 | translate([-i*2,-1,12]) cube([1.15,1.5,15],center=true); 36 | } 37 | } 38 | 39 | module belt() 40 | { 41 | difference() 42 | { 43 | union() 44 | { 45 | translate ([0,0,1]) cube([10,45,2],center=true); 46 | translate([0,23,1]) cylinder(r=5,h=2,center=true,$fn=16); 47 | translate([0,-23,1]) cylinder(r=5,h=2,center=true,$fn=16); 48 | } 49 | union() 50 | { 51 | // holes for belt 52 | translate([0,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 53 | translate([0,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 54 | } 55 | } 56 | difference() 57 | { 58 | translate ([0,0,18]) cube([10,38,36],center=true); 59 | union() 60 | { 61 | translate([0,0,20]) rotate([0,0,90]) x_carriage_beltcut(); 62 | // holes 63 | translate([0,-8,10]) rotate([0,90,0]) cylinder(r=6,h=100,center=true,$fn=6); 64 | translate([0,8,10]) rotate([0,90,0]) cylinder(r=6,h=100,center=true,$fn=6); 65 | } 66 | } 67 | } 68 | 69 | module x_carriage_base() 70 | { 71 | cylinder (r=diameter/2, h=2,center=true,$fn=80); 72 | translate([0,0,2]) border(); 73 | 74 | // bearing bases 75 | translate([rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 76 | translate([-rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 77 | 78 | //belt(); 79 | } 80 | 81 | module x_carriage_holes() 82 | { 83 | // bearing holes 84 | translate([rod_separation/2,0,-2]) horizontal_bearing_holes(); 85 | translate([-rod_separation/2,0,-2]) horizontal_bearing_holes(); 86 | 87 | // holes for belt 88 | translate([7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 89 | translate([7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 90 | translate([-7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 91 | translate([-7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 92 | 93 | 94 | // M3 holes 95 | translate([0,-30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 96 | translate([15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 97 | translate([-15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 98 | translate([0,30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 99 | translate([15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 100 | translate([-15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 101 | 102 | translate([rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 103 | translate([rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 104 | translate([-rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 105 | translate([-rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 106 | } 107 | 108 | 109 | module border_pusher() 110 | { 111 | difference() 112 | { 113 | cylinder (r=diameter/2, h=12.5,$fn=80); 114 | cylinder (r=diameter/2-5, h=15,$fn=80); 115 | } 116 | } 117 | 118 | module x_carriage_pusher_base() 119 | { 120 | cylinder (r=diameter/2, h=2,$fn=80); 121 | translate([0,0,0]) border_pusher(); 122 | 123 | translate([30,0,5.5]) cube([10,5,11],center=true); 124 | rotate([0,0,25])translate([30,0,5.5]) cube([10,5,11],center=true); 125 | rotate([0,0,-25])translate([30,0,5.5]) cube([10,5,11],center=true); 126 | 127 | } 128 | 129 | 130 | module x_carriage_pusher_holes() 131 | { 132 | cylinder (r=diameter/2-14, h=50,center=true,$fn=80); 133 | 134 | // M3 holes 135 | translate([0,-30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 136 | translate([15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 137 | translate([-15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 138 | translate([0,30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 139 | translate([15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 140 | translate([-15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 141 | 142 | translate([rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 143 | translate([rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 144 | translate([-rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 145 | translate([-rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 146 | 147 | // holes for belt 148 | translate([7.5,23,0]) cylinder(r=4,h=20,center=true,$fn=8); 149 | translate([7.5,-23,0]) cylinder(r=4,h=20,center=true,$fn=8); 150 | translate([-7.5,23,0]) cylinder(r=4,h=20,center=true,$fn=8); 151 | translate([-7.5,-23,0]) cylinder(r=4,h=20,center=true,$fn=8); 152 | } 153 | 154 | 155 | /* 156 | difference() 157 | { 158 | x_carriage_base(); 159 | x_carriage_holes(); 160 | } 161 | */ 162 | /* 163 | difference() 164 | { 165 | x_carriage_pusher_base(); 166 | x_carriage_pusher_holes(); 167 | } 168 | */ 169 | 170 | belt(); 171 | 172 | 173 | 174 | 175 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_x_carriage_pusher.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // X axis carriage base 6 | 7 | diameter = 80; 8 | rod_separation=45; 9 | bearing_diameter=15.5; 10 | bearing_length=26; 11 | 12 | 13 | module border() 14 | { 15 | difference() 16 | { 17 | cylinder (r=diameter/2, h=4.5,center=true,$fn=80); 18 | cylinder (r=diameter/2-4.5, h=6,center=true,$fn=80); 19 | } 20 | } 21 | 22 | module x_carriage_beltcut(){ 23 | // Cut in the middle for belt 24 | translate([-5.5+1,16.5,8]) cube([8,25,15]); 25 | // Cut clearing space for the belt 26 | //translate([-38,5,8]) cube([40,13,15]); 27 | // Belt slit 28 | translate([-32,16,8]) cube([80,1.25,15]); 29 | // Smooth entrance 30 | translate([-30,16.5,15.3]) rotate([45,0,0]) cube([80,15,15]); 31 | // Smooth the block on the upper side 32 | //translate([-66,27,15]) rotate([30,0,0]) cube([80,15,15]); 33 | // Teeth cuts (GT2) 34 | for ( i = [0 : 32] ){ 35 | translate([30-i*2,14.5,8]) cube([1.15,2,15]); 36 | } 37 | } 38 | 39 | module belt() 40 | { 41 | difference() 42 | { 43 | translate ([9,0,8]) cube([12,38,16],center=true); 44 | translate([25,0,-2]) rotate([0,0,90]) x_carriage_beltcut(); 45 | } 46 | } 47 | 48 | module x_carriage_base() 49 | { 50 | cylinder (r=diameter/2, h=2,center=true,$fn=80); 51 | translate([0,0,2]) border(); 52 | 53 | // bearing bases 54 | translate([rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 55 | translate([-rod_separation/2,0,5]) cube([20,bearing_length,10],center=true); 56 | 57 | //belt(); 58 | } 59 | 60 | module x_carriage_holes() 61 | { 62 | // bearing holes 63 | translate([rod_separation/2,0,-2]) horizontal_bearing_holes(); 64 | translate([-rod_separation/2,0,-2]) horizontal_bearing_holes(); 65 | 66 | // holes for belt 67 | translate([7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 68 | translate([7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 69 | translate([-7.5,23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 70 | translate([-7.5,-23,0]) cylinder(r=1.65,h=20,center=true,$fn=8); 71 | 72 | 73 | // M3 holes 74 | translate([0,-30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 75 | translate([15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 76 | translate([-15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 77 | translate([0,30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 78 | translate([15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 79 | translate([-15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 80 | 81 | translate([rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 82 | translate([rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 83 | translate([-rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 84 | translate([-rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 85 | } 86 | 87 | 88 | module border_pusher() 89 | { 90 | difference() 91 | { 92 | cylinder (r=diameter/2, h=12.5,$fn=80); 93 | cylinder (r=diameter/2-5, h=15,$fn=80); 94 | } 95 | } 96 | 97 | module x_carriage_pusher_base() 98 | { 99 | cylinder (r=diameter/2, h=2,$fn=80); 100 | translate([0,0,0]) border_pusher(); 101 | 102 | translate([30,0,5.5]) cube([10,5,11],center=true); 103 | rotate([0,0,25])translate([30,0,5.5]) cube([10,5,11],center=true); 104 | rotate([0,0,-25])translate([30,0,5.5]) cube([10,5,11],center=true); 105 | 106 | } 107 | 108 | 109 | module x_carriage_pusher_holes() 110 | { 111 | cylinder (r=diameter/2-14, h=50,center=true,$fn=80); 112 | 113 | // M3 holes 114 | translate([0,-30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 115 | translate([15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 116 | translate([-15,-28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 117 | translate([0,30,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 118 | translate([15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 119 | translate([-15,28,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 120 | 121 | translate([rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 122 | translate([rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 123 | translate([-rod_separation/2,-20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 124 | translate([-rod_separation/2,20,0]) cylinder(r=1.7,h=20,center=true,$fn=8); 125 | 126 | // holes for belt 127 | translate([7.5,23,0]) cylinder(r=4,h=20,center=true,$fn=8); 128 | translate([7.5,-23,0]) cylinder(r=4,h=20,center=true,$fn=8); 129 | translate([-7.5,23,0]) cylinder(r=4,h=20,center=true,$fn=8); 130 | translate([-7.5,-23,0]) cylinder(r=4,h=20,center=true,$fn=8); 131 | } 132 | 133 | 134 | /* 135 | difference() 136 | { 137 | x_carriage_base(); 138 | x_carriage_holes(); 139 | } 140 | */ 141 | 142 | difference() 143 | { 144 | x_carriage_pusher_base(); 145 | x_carriage_pusher_holes(); 146 | } 147 | 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_x_motor_mount.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // X axis motor mount 6 | 7 | 8 | motor_center_x = 22.5; 9 | motor_center_y = -22.5; 10 | base_height = 2.5; 11 | 12 | 13 | module x_motor_mount_base() 14 | { 15 | translate([0,-46,0]) cube([45,46,base_height]); // Motor mount 16 | 17 | // mount part 18 | translate([0,0,0]) cube([motor_center_x*2,20,base_height]); 19 | 20 | // Reinforce 21 | //translate([2.5,-20,0]) cube([2,42,base_height*1.5]); 22 | //translate([16,-6,0]) cube([2,28,base_height*1.5]); 23 | //translate([32,-6,0]) cube([2,28,base_height*1.5]); 24 | //translate([45.5,-20,0]) cube([2,42,base_height*1.5]); 25 | 26 | } 27 | 28 | module x_motor_mount_holes() 29 | { 30 | // Center motor hole 31 | translate([motor_center_x,motor_center_y,2]) cylinder(r=13,h=5,center=true,$fn=16); 32 | // Motor holes 33 | translate([motor_center_x-15.5,motor_center_y-15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 34 | translate([motor_center_x+15.5,motor_center_y-15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 35 | translate([motor_center_x-15.5,motor_center_y+15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 36 | translate([motor_center_x+15.5,motor_center_y+15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 37 | 38 | 39 | // Mount holes 40 | translate([motor_center_x-12.5,12.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 41 | translate([motor_center_x+12.5,12.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 42 | 43 | // motor mount cuts 44 | translate([motor_center_x*2+25,-motor_center_x*4+3,-1]) rotate([0,0,45]) cube([50,50,4]); // Motor mount 45 | translate([-25,-motor_center_x*4+3,-1]) rotate([0,0,45]) cube([50,50,4]); // Motor mount 46 | 47 | } 48 | 49 | 50 | 51 | 52 | 53 | translate([0,0,0]) rotate ([0,0,0]) difference() 54 | { 55 | x_motor_mount_base(); 56 | x_motor_mount_holes(); 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_y_axis_v2.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // Y axis parts (motot mount & end part) 6 | 7 | 8 | motor_center_x = 24.5; 9 | motor_center_y = -22; 10 | separation = 14; // laterial separation to center of rod 11 | base_height = 2.5; 12 | 13 | y_end_part_pulley_separation = 22; 14 | y_end_height = 16; 15 | y_end_length = 22; 16 | 17 | 18 | module y_motor_part_base() 19 | { 20 | translate([0,-46,0]) cube([49,46,base_height]); // Motor mount 21 | 22 | // Rod holder part 23 | translate([0,0,0]) cube([motor_center_x*2,22,base_height]); 24 | 25 | // Reinforce 26 | translate([2.5,-20,0]) cube([2,42,base_height*1.5]); 27 | translate([15,-5,0]) cube([2,27,base_height*1.5]); 28 | translate([32,-5,0]) cube([2,27,base_height*1.5]); 29 | translate([44.5,-20,0]) cube([2,42,base_height*1.5]); 30 | 31 | // Rod holder part 32 | rod_holder_base(); 33 | 34 | } 35 | 36 | module y_motor_part_holes() 37 | { 38 | // Center motor hole 39 | translate([motor_center_x,motor_center_y,2]) cylinder(r=13,h=5,center=true,$fn=16); 40 | // Motor holes 41 | translate([motor_center_x-15.5,motor_center_y-15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 42 | translate([motor_center_x+15.5,motor_center_y-15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 43 | translate([motor_center_x-15.5,motor_center_y+15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 44 | translate([motor_center_x+15.5,motor_center_y+15.5,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 45 | 46 | 47 | // Mount holes 48 | translate([motor_center_x-15.5,10,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 49 | translate([motor_center_x+15.5,10,2]) cylinder(r=1.8,h=5,center=true,$fn=8); 50 | 51 | // motor mount cuts 52 | translate([motor_center_x*2+15,-motor_center_x*4+3,-1]) rotate([0,0,45]) cube([50,50,4]); // Motor mount 53 | translate([-15,-motor_center_x*4+3,-1]) rotate([0,0,45]) cube([50,50,4]); // Motor mount 54 | 55 | // Rod holder holes 56 | rod_holder_holes(); 57 | 58 | } 59 | 60 | module rod_holder_base() 61 | { 62 | translate([motor_center_x-9,0,0]) cube([18,18,6+separation+0.5]); 63 | } 64 | module rod_holder_holes() 65 | { 66 | translate([motor_center_x,9,separation]) rotate([90,0,0]) cylinder(r=4.1,h=20,$fn=24,center=true); // 8mm rod 67 | 68 | 69 | // Rod bar aperture 70 | translate([motor_center_x-0.5,-1,60]) rotate([0,90,0]) cube([50,50,1]); 71 | 72 | /* 73 | // Cube bevel 74 | translate([motor_center_x-9,16,base_height+0.1]) rotate([0,0,45]) cube([24,24,10+separation-base_height]); 75 | translate([motor_center_x,25,base_height+0.1]) rotate([0,0,-45]) cube([24,24,10+separation-base_height]); 76 | translate([motor_center_x-26,-15,base_height+0.1]) rotate([0,0,-45]) cube([24,24,10+separation-base_height]); 77 | translate([motor_center_x+15,-26,base_height+0.1]) rotate([0,0,45]) cube([24,24,10+separation-base_height]); 78 | */ 79 | } 80 | 81 | 82 | // *** Y_END *** 83 | module y_end_part_base() 84 | { 85 | cube([separation+8+6,12,y_end_height]); 86 | translate([0,12,0]) cube([6,y_end_part_pulley_separation,y_end_height]); 87 | translate([separation+8,12,0]) cube([6,y_end_part_pulley_separation,y_end_height]); 88 | 89 | //translate([0,0,12]) cube([2,25,10]); 90 | //translate([0,0,-10]) cube([2,25,10]); 91 | 92 | // Bearing 93 | //translate([6.25,12+end_part_pulley_separation,6]) rotate([0,90,0]) cylinder(r=5,h=0.5,$fn=16,center=true); 94 | //translate([13.75,12+end_part_pulley_separation,6]) rotate([0,90,0]) cylinder(r=5,h=0.5,$fn=16,center=true); 95 | 96 | } 97 | 98 | module y_end_part_holes() 99 | { 100 | // CUTS 101 | translate([separation-0.5,-1,y_end_height/2-5]) cube([1,20,10]); 102 | translate([separation-5,-1,y_end_height/2]) cube([10,20,1]); 103 | 104 | translate([separation,0,y_end_height/2]) rotate([90,0,0]) cylinder(r=4.05,h=25,$fn=16,center=true); 105 | // Bearing hole 8mm (608) 106 | translate([separation,12+y_end_part_pulley_separation,y_end_height/2]) rotate([0,90,0]) cylinder(r=4,h=separation+8+4,$fn=32,center=true); 107 | 108 | //translate([0,12+y_end_part_pulley_separation,0]) cube([20,10,50]); 109 | 110 | translate([0,16,y_end_height/2]) rotate([0,90,0]) cylinder(r=1.6,h=50,$fn=12,center=true); 111 | translate([0,25,y_end_height/2]) rotate([0,90,0]) cylinder(r=1.6,h=50,$fn=12,center=true); 112 | 113 | translate([y_end_length,16,y_end_height/2]) rotate([0,90,0]) cylinder(r=3.9,h=35,$fn=16,center=true); 114 | translate([y_end_length,25,y_end_height/2]) rotate([0,90,0]) cylinder(r=3.9,h=35,$fn=16,center=true); 115 | } 116 | 117 | 118 | translate([0,0,0]) rotate ([0,0,0]) difference() 119 | { 120 | y_motor_part_base(); 121 | y_motor_part_holes(); 122 | } 123 | 124 | 125 | translate([-30,-20,0]) difference() 126 | { 127 | y_end_part_base(); 128 | y_end_part_holes(); 129 | } 130 | 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/AHR_y_carriage.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // Y axis carriage and tube end part 6 | // This part is based on ideas of the prusa i3 carriage 7 | 8 | bearing_diameter=15.5; 9 | 10 | module horizontal_bearing_holes(bearings=1){ 11 | cutter_lenght = 4+bearings*25; 12 | one_holder_lenght = 8+25; 13 | holder_lenght = 8+bearings*25; 14 | 15 | // Main bearing cut 16 | difference(){ 17 | translate(v=[0,0,12]) rotate(a=[90,0,0]) translate(v=[0,0,-cutter_lenght/2]) cylinder(h = cutter_lenght, r=bearing_diameter/2, $fn=64); 18 | // Bearing retainers 19 | translate(v=[0,-holder_lenght/2,3]) cube(size = [24,6,34], center = true); 20 | translate(v=[0,+holder_lenght/2,3]) cube(size = [24,6,34], center = true); 21 | } 22 | 23 | // Ziptie cutouts 24 | 25 | ziptie_cut_ofset = 0; 26 | for ( i = [0 : bearings-1] ){ 27 | // For easier positioning I move them by half of one 28 | // bearing holder then add each bearign lenght and then center again 29 | translate(v=[0,-holder_lenght/2,0]) translate(v=[0,one_holder_lenght/2+i*25,0]) difference(){ 30 | union(){ 31 | translate(v=[0,2-6,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 4, r=12.5, $fn=50); 32 | translate(v=[0,2+6,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 4, r=12.5, $fn=50); 33 | } 34 | translate(v=[0,10,12]) rotate(a=[90,0,0]) translate(v=[0,0,0]) cylinder(h = 24, r=9.5, $fn=50); 35 | } 36 | } 37 | 38 | 39 | } 40 | 41 | module x_carriage_base(){ 42 | 43 | // Bearings 44 | translate([29,-10.5,0]) rotate([0,0,90]) cube([24,58,10]); 45 | translate([29,4.6,0]) rotate([0,0,90]) cube([8.5,58,15]); 46 | // Base plate 47 | translate([-30,-10.5,0]) cube([60,70,6]); 48 | // Belt block 49 | translate([-30,12,0]) cube([60,10,17]); 50 | // Rods block 51 | translate([-30,45,0]) cube([60,15,17]); 52 | } 53 | 54 | module x_carriage_beltcut(){ 55 | // Cut in the middle for belt 56 | translate([-5.5+1,16.5,8]) cube([8,25,15]); 57 | // Cut clearing space for the belt 58 | //translate([-38,5,8]) cube([40,13,15]); 59 | // Belt slit 60 | translate([-32,16,8]) cube([80,1.25,15]); 61 | // Smooth entrance 62 | translate([-30,16.5,15.3]) rotate([45,0,0]) cube([80,15,15]); 63 | // Smooth the block on the upper side 64 | //translate([-66,27,15]) rotate([30,0,0]) cube([80,15,15]); 65 | // Teeth cuts (GT2) 66 | for ( i = [0 : 32] ){ 67 | translate([30-i*2,14.5,8]) cube([1.15,2,15]); 68 | } 69 | } 70 | 71 | module x_carriage_holes(){ 72 | // Small bearing holder holes cutter 73 | translate([0,12,12]) rotate([0,-90,90]) horizontal_bearing_holes(2); 74 | // Main rod hole 75 | translate([0,0,12]) rotate([0,90,0]) cylinder(r=5.8,h=120,$fn=64,center=true); 76 | // Rods holes 77 | translate([-45/2,52,0]) cylinder(r=4.1,h=50,$fn=16,center=true); 78 | translate([45/2,52,0]) cylinder(r=4.1,h=50,$fn=16,center=true); 79 | // Pulley hole 80 | translate([0,52,10.5]) rotate([90,0,0]) cylinder(r=4.08,h=20,$fn=16,center=true); 81 | // Motor holes 82 | translate([-25/2,52,0]) cylinder(r=1.7,h=50,$fn=16,center=true); 83 | translate([25/2,52,0]) cylinder(r=1.7,h=50,$fn=16,center=true); 84 | translate([-25/2,40,0]) cylinder(r=1.7,h=50,$fn=16,center=true); 85 | translate([25/2,40,0]) cylinder(r=1.7,h=50,$fn=16,center=true); 86 | translate([-25/2,52,10]) rotate([90,0,0]) cylinder(r=1.7,h=20,$fn=16,center=true); 87 | translate([25/2,52,10]) rotate([90,0,0]) cylinder(r=1.7,h=20,$fn=16,center=true); 88 | 89 | translate([0,52,0]) cylinder(r=1.65,h=50,$fn=16,center=true); 90 | } 91 | 92 | module x_carriage_fancy() 93 | { 94 | translate([-50,-23,-16]) rotate([30,0,0]) cube([100,20,20]); 95 | translate([-50,-20,-1]) rotate([45,0,0]) cube([100,20,20]); 96 | translate([-50,59,0]) rotate([-30,0,0]) cube([100,20,20]); 97 | translate([-50,58,17.5]) rotate([-45,0,0]) cube([100,20,20]); 98 | 99 | translate([33,52,20]) rotate([0,-30,0]) cube([10,20,10],center=true); 100 | translate([-33,52,20]) rotate([0,30,0]) cube([10,20,10],center=true); 101 | 102 | translate([-32,-11,10]) rotate([90,0,45]) cube([10,40,10],center=true); 103 | translate([32,-11,10]) rotate([90,0,45]) cube([10,40,10],center=true); 104 | translate([-32,62.5,10]) rotate([90,0,45]) cube([10,40,10],center=true); 105 | translate([32,62.5,10]) rotate([90,0,45]) cube([10,40,10],center=true); 106 | } 107 | 108 | module y_carriage_rod_ends_base() 109 | { 110 | // Rods block 111 | translate([0,0,0.75]) cube([45,14,1.5],center=true); 112 | translate([22,0,0.75]) cylinder(r=7,h=1.5,$fn=16,center=true); 113 | translate([-22,0,0.75]) cylinder(r=7,h=1.5,$fn=16,center=true); 114 | } 115 | 116 | module y_carriage_rod_ends_holes() 117 | { 118 | // M3 holes 119 | translate([-25/2,0,0]) cylinder(r=1.75,h=50,$fn=16,center=true); 120 | translate([25/2,0,0]) cylinder(r=1.75,h=50,$fn=16,center=true); 121 | 122 | translate([-25/2,0,0.75]) cylinder(r=3.2,h=10,$fn=16); 123 | translate([25/2,0,0.75]) cylinder(r=3.2,h=10,$fn=16); 124 | 125 | 126 | // center hole 127 | cylinder(r=4.5,h=10,$fn=6,center=true); 128 | 129 | 130 | } 131 | 132 | // Final part 133 | module x_carriage(){ 134 | translate ([0,0,0]) difference(){ 135 | x_carriage_base(); 136 | x_carriage_beltcut(); 137 | x_carriage_holes(); 138 | x_carriage_fancy(); 139 | } 140 | } 141 | 142 | x_carriage(); 143 | 144 | // tube ends 145 | translate ([40,20,0])rotate([0,0,90]) difference() 146 | { 147 | y_carriage_rod_ends_base(); 148 | y_carriage_rod_ends_holes(); 149 | } 150 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/PrintableBearing.scad: -------------------------------------------------------------------------------- 1 | // PART: http://www.thingiverse.com/thing:174353 2 | // AntonioNav 3 | // LM8UU for Print in PLA 4 | 5 | $fn=80; 6 | 7 | radio_ext=15/2; 8 | radio_int=7.95/2; //Para varilla M8 9 | altura=23.8; 10 | grosor=0.9; 11 | num_dientes=9; 12 | 13 | 14 | module varilla_lisa_M8(){ 15 | cylinder(r=4,h=70); 16 | } 17 | 18 | module diente(){ 19 | d_ancho=1; 20 | d_largo=radio_ext-d_ancho-0.2-radio_int; 21 | 22 | translate([0,-radio_int,0]) 23 | hull(){ 24 | translate([0,-d_ancho/2,0]) 25 | cylinder(r=d_ancho/2,h=altura); 26 | translate([0,-d_largo,0]) 27 | cylinder(r=d_ancho+0.2,h=altura); 28 | } 29 | 30 | } 31 | 32 | module lm8uu(){ 33 | 34 | difference(){ 35 | cylinder(r=radio_ext,h=altura); 36 | translate([0,0,-1]) 37 | cylinder(r=radio_ext-grosor,h=altura+2); 38 | } 39 | for (i=[0:(num_dientes-1)]){ 40 | rotate(i*360/num_dientes,[0,0,1]) 41 | diente(); 42 | } 43 | } 44 | 45 | //color("blue") varilla_lisa_M8(); 46 | lm8uu(); 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/puck_v2.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // PUCK 6 | 7 | 8 | // PUCK v2 (lighter version) 9 | difference() 10 | { 11 | cylinder(r=30,h=5,center=false,$fn=100); 12 | translate([0,0,-1]) cylinder(r=27.25,h=3,center=false,$fn=64); 13 | translate([0,0,2.8]) cylinder(r=27.25,h=10,center=false,$fn=64); 14 | } 15 | 16 | 17 | /* 18 | // PUSHER 19 | difference() 20 | { 21 | cylinder(r=40,h=18,center=false,$fn=100); 22 | translate([0,0,10]) cylinder(r=35,h=20,center=false,$fn=64); 23 | } 24 | cylinder(r=20,h=40,center=false,$fn=50); 25 | translate([0,0,40]) sphere(r=20,$fn=50); 26 | */ 27 | 28 | -------------------------------------------------------------------------------- /3DPrintable_parts/OPENSCAD/pusher.scad: -------------------------------------------------------------------------------- 1 | // AIR HOCKEY ROBOT PROJECT (AHROBOT) 2 | // 3 | // by Jose Julio 4 | 5 | // PUSHER 6 | 7 | /* 8 | // PUCK v2 (lighter version) 9 | difference() 10 | { 11 | cylinder(r=30,h=5,center=false,$fn=100); 12 | translate([0,0,-1]) cylinder(r=27.25,h=3,center=false,$fn=64); 13 | translate([0,0,2.8]) cylinder(r=27.25,h=10,center=false,$fn=64); 14 | } 15 | */ 16 | 17 | 18 | // PUSHER 19 | difference() 20 | { 21 | cylinder(r=40,h=18,center=false,$fn=100); 22 | translate([0,0,10]) cylinder(r=35,h=20,center=false,$fn=64); 23 | } 24 | cylinder(r=20,h=40,center=false,$fn=50); 25 | translate([0,0,40]) sphere(r=20,$fn=50); 26 | 27 | 28 | -------------------------------------------------------------------------------- /Arduino/AHRobot/AHRobot.ino: -------------------------------------------------------------------------------- 1 | // AHR: AIR HOCKEY ROBOT PROJECT 2 | // Author: Jose Julio (@jjdrones) 3 | // Hardware: Arduino MEGA + Ramps 1.4 4 | // Date: 18/11/2013 5 | // Last updated: 24/03/2014 6 | // Version: 1.05 7 | // Project page : 8 | // Spanish: http://cienciaycacharreo.blogspot.com.es/2014/02/nuevo-proyecto-air-hockey-robot-3d.html 9 | // English: http://cienciaycacharreo.blogspot.com.es/2014/02/new-project-air-hockey-robot-3d-printer.html 10 | // GIT repository: https://github.com/JJulio/AHRobot 11 | // License: Open Software GPL License 12 | 13 | // ROBOT and USER configuration parameters 14 | #include "Configuration.h" 15 | #include "Definitions.h" // Variable definitions 16 | 17 | void setup() 18 | { 19 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 20 | // X MOTOR 21 | // X-STEP: A0 (PF0) 22 | // X-DIR: A1 (PF1) 23 | // X-ENABLE: D38 (PD7) 24 | // Y MOTOR (Y-LEFT) 25 | // Y-STEP: A6 (PF6) 26 | // Y-DIR: A7 (PF7) 27 | // Y-ENABLE: A2 (PF2) 28 | // Z MOTOR (Y-RIGHT) 29 | // Z-STEP: D46 (PL3) 30 | // Z-DIR: D48 (PL1) 31 | // Z-ENABLE: A8 (PK0) 32 | 33 | // STEPPER PINS 34 | // X_AXIS 35 | pinMode(38,OUTPUT); // ENABLE MOTOR 36 | pinMode(A0,OUTPUT); // STEP MOTOR 37 | pinMode(A1,OUTPUT); // DIR MOTOR 38 | // Y_AXIS (Y-LEFT) 39 | pinMode(A2,OUTPUT); // ENABLE MOTOR 40 | pinMode(A6,OUTPUT); // STEP MOTOR 41 | pinMode(A7,OUTPUT); // DIR MOTOR 42 | // Z_AXIS (Y-RIGHT) 43 | pinMode(A8,OUTPUT); // ENABLE MOTOR 44 | pinMode(46,OUTPUT); // STEP MOTOR 45 | pinMode(48,OUTPUT); // DIR MOTOR 46 | 47 | pinMode(A3,OUTPUT); // DEBUG PIN FOR OSCILLOSCOPE TIME MEASURES 48 | 49 | pinMode(19,INPUT); // RX1 Serial Port 1 50 | pinMode(18,OUTPUT); // TX1 51 | 52 | //FANS and LEDS 53 | pinMode(8,OUTPUT); 54 | pinMode(9,OUTPUT); 55 | pinMode(10,OUTPUT); 56 | pinMode(13,OUTPUT); 57 | 58 | // Disable Motors 59 | digitalWrite(38,HIGH); 60 | digitalWrite(A2,HIGH); 61 | digitalWrite(A8,HIGH); 62 | 63 | Serial.begin(115200); 64 | Serial.println("AHR Robot version 1.05"); 65 | delay(500); 66 | Serial.println("Initializing robot..."); 67 | delay(2000); 68 | 69 | cameraProcessInit(); // Camera variable initializations 70 | 71 | // Robot positions initialization 72 | defense_position = ROBOT_DEFENSE_POSITION; // Robot y axis defense position 73 | attack_position = ROBOT_DEFENSE_ATTACK_POSITION; // Robot y axis position for defense+attack 74 | 75 | //LED blink 76 | for (uint8_t k=0;k<4;k++) 77 | { 78 | digitalWrite(13,HIGH); 79 | delay(300); 80 | digitalWrite(13,LOW); 81 | delay(300); 82 | } 83 | 84 | // We use TIMER 1 for stepper motor X AXIS and Timer 3 for Y AXIS 85 | // STEPPER MOTORS INITIALIZATION 86 | // TIMER1 CTC MODE 87 | TCCR1B &= ~(1<=1000) // 1Khz loop 183 | { 184 | //dt = (timer_value-timer_old)/1000; 185 | timer_old = timer_value; 186 | 187 | loop_counter++; 188 | 189 | packetRead(); 190 | if (newPacket) 191 | { 192 | //dt = (timer_value-timer_packet_old)/1000.0; 193 | dt = 16; //60 Hz = 16.66ms 194 | timer_packet_old = timer_value; 195 | newPacket=0; 196 | logOutput = 1; 197 | //Serial.print("P"); 198 | 199 | // Puck detection and trayectory prediction 200 | cameraProcess(puckPixX,puckPixY,dt); 201 | 202 | // Robot position detection for missing steps detection in steppers. 203 | robotDetection(robotPixX,robotPixY); 204 | missingStepsDetection(); 205 | 206 | // Robot strategy based on puck prediction 207 | newDataStrategy(); 208 | 209 | 210 | } // End packet received 211 | 212 | 213 | 214 | // Strategy : Robot behaviour 215 | robotStrategy(); 216 | 217 | // Console output 218 | if (logOutput) 219 | { 220 | logOutput = 0; 221 | Serial.print(robot_status); 222 | Serial.print(" "); 223 | Serial.print(com_pos_x); 224 | Serial.print(" "); 225 | Serial.print(com_pos_y); 226 | Serial.print(" "); 227 | Serial.println(speed_y); 228 | } 229 | // DEBUG : PUCK POSITION 230 | if (loop_counter == 4000) 231 | { 232 | Serial.print("PUCK POSITION: "); 233 | Serial.print(puckCoordX); 234 | Serial.print(","); 235 | Serial.println(puckCoordY); 236 | } 237 | 238 | // DEBUG: We inform of the position error of the robot as seen in the camera (util for calibrations) 239 | if (loop_counter == 4002) 240 | { 241 | Serial.print("ROBOT ERROR: "); 242 | Serial.print(robotMissingStepsErrorX); 243 | Serial.print(","); 244 | Serial.println(robotMissingStepsErrorY); 245 | } 246 | 247 | 248 | positionControl(); 249 | } // 1Khz loop 250 | } 251 | 252 | 253 | 254 | 255 | 256 | -------------------------------------------------------------------------------- /Arduino/AHRobot/Camera.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // Camera serial packet read and Puck trayectory prediction 4 | // Robot position detection and missing steps check control 5 | 6 | uint16_t extractParamInt(uint8_t pos){ 7 | union{ 8 | unsigned char Buff[2]; 9 | uint16_t d; 10 | } 11 | u; 12 | 13 | u.Buff[0] = (unsigned char)SBuffer[pos]; 14 | u.Buff[1] = (unsigned char)SBuffer[pos+1]; 15 | return(u.d); 16 | } 17 | 18 | // Read Vision System Packets over Serial Port 19 | void packetRead() 20 | { 21 | unsigned char i; 22 | if (Serial.available() > 0) { 23 | //Serial.println("P"); 24 | // We rotate the Buffer (we could implement a ring buffer in future) 25 | for (i=11;i>0;i--){ 26 | SBuffer[i] = SBuffer[i-1]; 27 | } 28 | SBuffer[0] = Serial.read(); 29 | //Serial.print(S1Buffer[0]); 30 | // We look for a message start like "mm" to sync packets 31 | if ((SBuffer[0] == 'm')&&(SBuffer[1] == 'm')) 32 | { 33 | if (readStatus == 0) 34 | { 35 | readStatus=1; 36 | readCounter=12; 37 | } 38 | else 39 | { 40 | Serial.println("S ERR"); 41 | readStatus=1; 42 | readCounter=12; 43 | } 44 | return; 45 | } 46 | else if (readStatus==1) 47 | { 48 | readCounter--; // Until we complete the packet 49 | if (readCounter<=0) // packet complete!! 50 | { 51 | // Extract parameters 52 | cam_timestamp = extractParamInt(10); 53 | puckPixX = extractParamInt(8); 54 | puckPixY = extractParamInt(6); 55 | puckSize = extractParamInt(4); 56 | robotPixX = extractParamInt(2); 57 | robotPixY = extractParamInt(0); 58 | readStatus = 0; 59 | newPacket = 1; 60 | //Serial.println("P"); 61 | } 62 | } 63 | } 64 | } 65 | 66 | 67 | // This process takes the puck position from camera and calculate puck position in robot reference system 68 | // and trajectory prediction. time in ms 69 | void cameraProcess(int posX, int posY, int time) 70 | { 71 | int coordX; 72 | int coordY; 73 | int vectorX; 74 | int vectorY; 75 | double slope; 76 | 77 | int bounce_x; 78 | int bounce_y; 79 | 80 | // Convert from Camera reference system to Robot reference system 81 | // We suppose very small camera angle rotatios (less than 5 degrees) so we use the 82 | // aproximation that sin(cam_rotation) = cam_rotation (in radians) 83 | // Camera X axis correspond to robot Y axis 84 | coordY = (posX - cam_center_x); // First we convert image coordinates to center of image 85 | coordX = (posY - cam_center_y); 86 | 87 | coordY = ROBOT_CENTER_Y - coordY*cam_pix_to_mm; 88 | coordX = ROBOT_CENTER_X - coordX*cam_pix_to_mm*(1-cam_rotation); 89 | 90 | // Speed calculation on each axis 91 | vectorX = (coordX-puckCoordX); 92 | vectorY = (coordY-puckCoordY); 93 | 94 | puckOldCoordX = puckCoordX; 95 | puckOldCoordY = puckCoordY; 96 | puckCoordX = coordX; 97 | puckCoordY = coordY; 98 | 99 | // Noise detection, if there are a big vector this should be noise 100 | if ((vectorY<-160)||(vectorY>160)||(vectorX>160)||(vectorX<-160)) 101 | { 102 | Serial.println("NOISE"); 103 | predict_status = -1; 104 | predict_x_old = -1; 105 | return; 106 | } 107 | 108 | puckOldSpeedX = puckSpeedX; 109 | puckOldSpeedY = puckSpeedY; 110 | puckSpeedX = vectorX*100/time; // speed in dm/ms (we use this units to not overflow the variable) 111 | puckSpeedY = vectorY*100/time; 112 | if (predict_status == -1) // Noise on last reading? 113 | { 114 | puckSpeedXAverage = puckSpeedX; 115 | puckSpeedYAverage = puckSpeedY; 116 | } 117 | else 118 | { 119 | if (myAbs(puckSpeedX - puckOldSpeedX)<50) 120 | puckSpeedXAverage = (puckSpeedX + puckOldSpeedX)>>1; 121 | else 122 | puckSpeedXAverage = puckSpeedX; 123 | if (myAbs(puckSpeedY - puckOldSpeedY)<50) 124 | puckSpeedYAverage = (puckSpeedY + puckOldSpeedY)>>1; 125 | else 126 | puckSpeedYAverage = puckSpeedY; 127 | } 128 | 129 | //puckSpeed = sqrt(vectorX*vectorX + vectorY*vectorY)*1000.0/time; 130 | //puckDirection = atan2(vectorY,vectorX); 131 | 132 | predict_x_attack = -1; 133 | 134 | // It´s time to predict... 135 | // Based on actual position and move vector we need to know the future... 136 | // Posible impact? speed Y is negative when the puck is moving to the robot 137 | if (puckSpeedYAverage<-40) 138 | { 139 | predict_status = 1; 140 | // Puck is comming... 141 | // We need to predict the puck position when it reaches our goal Y position = defense_position 142 | // slope formula: m = (y2-y1)/(x2-x1) 143 | if (vectorX == 0) // To avoid division by 0 144 | slope = 9999999; 145 | else 146 | slope = (float)vectorY/(float)vectorX; 147 | 148 | // x = (y2-y1)/m + x1 149 | predict_y = defense_position; 150 | predict_x = (predict_y-coordY)/slope; 151 | predict_x += coordX; 152 | 153 | predict_x_attack = (attack_position-coordY)/slope; 154 | predict_x_attack += coordX; 155 | 156 | // puck has a bounce with side wall? 157 | if ((predict_x<38)||(predict_x>562)) 158 | { 159 | predict_status = 2; 160 | predict_bounce = 1; 161 | predict_bounce_status = 1; 162 | // We start a new prediction 163 | // Wich side? 164 | if (predict_x<38) 165 | { 166 | //Left side. We calculare the impact point 167 | bounce_x = 38; 168 | } 169 | else 170 | { 171 | //Right side. We calculare the impact point 172 | bounce_x = 562; 173 | } 174 | bounce_y = (bounce_x - coordX)*slope + coordY; 175 | predict_time = (bounce_y-puckCoordY)*100L/puckSpeedY; // time until bouce 176 | // bounce prediction => slope change with the bounce, we only need to change the sign, easy!! 177 | slope = -slope; 178 | predict_y = defense_position; 179 | predict_x = (predict_y-bounce_y)/slope; 180 | predict_x += bounce_x; 181 | 182 | if ((predict_x<38)||(predict_x>562)) 183 | { 184 | // New bounce?? 185 | // We do nothing then... with two bounces there are small risk of goal... 186 | predict_x_old = -1; 187 | predict_status = 0; 188 | } 189 | else 190 | { 191 | // one side bounce 192 | // If the puckSpeedY has changed a lot this mean that the puck has touch one side 193 | if (myAbs(puckSpeedY - puckOldSpeedY) > 50) 194 | { 195 | // We dont make a new prediction... 196 | } 197 | else 198 | { 199 | // average of the results (some noise filtering) 200 | if (predict_x_old != -1) 201 | predict_x = (predict_x_old + predict_x)>>1; 202 | predict_x_old = predict_x; 203 | // We introduce a factor (130 instead of 100) to model the bounce (30% loss in speed)(to improcve...) 204 | predict_time = predict_time + (predict_y-puckCoordY)*130L/puckSpeedY; // in ms 205 | } 206 | } 207 | } 208 | else // No bounce, direct impact 209 | { 210 | if (predict_bounce_status == 1) // This is the first direct impact trajectory after a bounce 211 | { 212 | // We dont predict nothing new... 213 | predict_bounce_status = 0; 214 | } 215 | else 216 | { 217 | // average of the results (some noise filtering) 218 | if (predict_x_old != -1) 219 | predict_x = (predict_x_old + predict_x)>>1; 220 | predict_x_old = predict_x; 221 | 222 | predict_time = (predict_y-puckCoordY)*100L/puckSpeedY; // in ms 223 | predict_time_attack = (attack_position-puckCoordY)*100L/puckSpeedY; // in ms 224 | } 225 | } 226 | } 227 | else // Puck is moving slowly or to the other side 228 | { 229 | predict_x_old = -1; 230 | predict_status = 0; 231 | predict_bounce = 0; 232 | predict_bounce_status = 0; 233 | } 234 | } 235 | 236 | // Return the predicted position of the puck in predict_time miliseconds 237 | int predictPuckXPosition(int predict_time) 238 | { 239 | return (puckCoordX + (long)puckSpeedXAverage*predict_time/100L); 240 | } 241 | 242 | // Return the predicted position of the puck in predict_time miliseconds 243 | int predictPuckYPosition(int predict_time) 244 | { 245 | return (puckCoordY + (long)puckSpeedYAverage*predict_time/100L); 246 | } 247 | 248 | // Initialization routine 249 | void cameraProcessInit() 250 | { 251 | // Default values 252 | cam_center_x = CAM_PIX_CENTER_X; 253 | cam_center_y = CAM_PIX_CENTER_Y; 254 | cam_pix_to_mm = CAM_PIX_TO_MM; 255 | cam_rotation = 0; //radians 1º = 0.01745 2º = 0.035 4º = 0.07 5º = 0.087 256 | 257 | predict_x_old = -1; 258 | } 259 | 260 | // Robot position detection. Transformation from camera reference system (in pixels) to robot reference system 261 | void robotDetection(int posX, int posY) 262 | { 263 | int coordX; 264 | int coordY; 265 | 266 | // Convert from Camera reference system to Robot reference system 267 | // We suppose very small angle rotatios (less than 5 degrees) so we use the 268 | // aproximation that sin cam_rotation = cam_rotation (in radians) 269 | // Camera X axis correspond to robot Y axis 270 | coordY = (posX - cam_center_x); // First we convert image coordinates to center of image 271 | coordX = (posY - cam_center_y); 272 | 273 | coordY = ROBOT_CENTER_Y - coordY*cam_pix_to_mm; 274 | coordX = ROBOT_CENTER_X - coordX*cam_pix_to_mm*(1-cam_rotation); 275 | 276 | // Valid coordinates? 277 | if (coordX>40&&coordX<560&&coordY>40&&coordY<500) 278 | { 279 | robotCoordX = coordX; 280 | robotCoordY = coordY; 281 | } 282 | else 283 | { 284 | robotCoordX = 0; 285 | robotCoordY = 0; 286 | } 287 | } 288 | 289 | // Function to detect missing steps in steppers 290 | // When the robot is stopped in a known position (defense position) we compare the estimated position from steppers with the position of the robot seen in the camera. 291 | void missingStepsDetection() 292 | { 293 | int robot_position_x_mm; 294 | int robot_position_y_mm; 295 | 296 | 297 | // If we are stopped and we have a good robot detection (camera) 298 | if ((speed_x == 0)&&(speed_y == 0)&&(robotCoordX != 0)) 299 | { 300 | robot_position_x_mm = position_x/X_AXIS_STEPS_PER_UNIT; 301 | robot_position_y_mm = position_y/Y_AXIS_STEPS_PER_UNIT; 302 | // Are we near center and near defense position? 303 | if ((robot_position_x_mm > (ROBOT_CENTER_X-40))&&(robot_position_x_mm < (ROBOT_CENTER_X+40))&&(robot_position_y_mm >= ROBOT_MIN_Y)&&(robot_position_y_mm < (ROBOT_DEFENSE_POSITION+30))) 304 | { 305 | robotCoordSamples++; 306 | robotCoordXAverage += robotCoordX; 307 | robotCoordYAverage += robotCoordY; 308 | // When we collect 10 samples we make the correction 309 | if (robotCoordSamples == 10) 310 | { 311 | // X axis 312 | robotCoordXAverage = robotCoordXAverage/robotCoordSamples; 313 | robotMissingStepsErrorX = myAbs(robot_position_x_mm - robotCoordXAverage); // in milimeters) 314 | if (robotMissingStepsErrorX > MISSING_STEPS_MAX_ERROR_X) 315 | { 316 | // Missing steps detected on X axis!! We need to correct this... 317 | #ifdef CORRECT_MISSING_STEPS_X 318 | position_x = robotCoordXAverage*X_AXIS_STEPS_PER_UNIT; 319 | Serial.print("MSX "); 320 | Serial.println(robotMissingStepsErrorX); 321 | #endif 322 | } 323 | // Y AXIS 324 | robotCoordYAverage = robotCoordYAverage/robotCoordSamples; 325 | robot_position_y_mm += ROBOT_POSITION_CAMERA_CORRECTION_Y; // correction because camera point of view and robot mark 326 | robotMissingStepsErrorY = myAbs(robot_position_y_mm - robotCoordYAverage); 327 | if (robotMissingStepsErrorY > MISSING_STEPS_MAX_ERROR_Y) 328 | { 329 | // Missing steps detected on Y axis!! We need to correct this... 330 | #ifdef CORRECT_MISSING_STEPS_Y 331 | position_y = robotCoordYAverage*Y_AXIS_STEPS_PER_UNIT; 332 | Serial.print("MSY "); 333 | Serial.println(robotMissingStepsErrorY); 334 | #endif 335 | } 336 | } 337 | } 338 | else 339 | { 340 | robotCoordSamples = 0; 341 | robotCoordXAverage = 0; 342 | robotCoordYAverage = 0; 343 | robotMissingStepsErrorX = 0; 344 | robotMissingStepsErrorY = 0; 345 | } 346 | } 347 | else 348 | { 349 | robotCoordSamples = 0; 350 | robotCoordXAverage = 0; 351 | robotCoordYAverage = 0; 352 | robotMissingStepsErrorX = 0; 353 | robotMissingStepsErrorY = 0; 354 | } 355 | } 356 | 357 | 358 | -------------------------------------------------------------------------------- /Arduino/AHRobot/Configuration.h: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // USER CONFIGURATIONS HERE 4 | // ROBOT DIMENSIONS, MAX SPEED, MAX ACCELERATION, CALIBRATION 5 | 6 | // THIS VALUES DEPENDS ON THE VOLTAGE, MOTORS, PULLEYS AND ROBOT CONSTRUCTION 7 | // RECOMMENDED VALUES FOR 12V POWER SUPPLY 8 | #define MIN_ACCEL_X 60 9 | #define MAX_ACCEL_X 300 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 10 | #define MIN_ACCEL_Y 60 11 | #define MAX_ACCEL_Y 145 //140//220 12 | #define MAX_SPEED_X 25000 //max 25000 for 12V // Maximun speed in steps/seg 13 | #define MAX_SPEED_Y 25000 14 | 15 | // RECOMMENDED VALUES FOR 15V POWER SUPPLY 16 | //#define MIN_ACCEL_X 40 17 | //#define MAX_ACCEL_X 380 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 18 | //#define MIN_ACCEL_Y 70 19 | //#define MAX_ACCEL_Y 200 //140//220 20 | //#define MAX_SPEED_X 28000 //max 25000 for 12V // Maximun speed in steps/seg 21 | //#define MAX_SPEED_Y 28000 22 | 23 | 24 | // This is for the Accel ramp implementation (to smooth the intial acceleration), simplified S-profile 25 | #define ACCEL_RAMP_MIN 2500 // The S profile is generated up to this speed 26 | #define ACCEL_RAMP_MAX 10000 27 | 28 | // UNCOMMENT THIS LINES TO INVERT MOTORS 29 | #define INVERT_X_AXIS 1 30 | //#define INVERT_Y_AXIS 1 //Y-LEFT 31 | //#define INVERT_Z_AXIS 1 //Y_RIGHT 32 | 33 | // Geometric calibration. 34 | // This depends on the pulley teeth. For 42 teeth GT2 => 19, for 40 teeth GT2 => 20, for 16 teeth T5 => 20 35 | #define X_AXIS_STEPS_PER_UNIT 19 // With 42 teeth GT2 pulley and 1/8 microstepping on drivers 36 | #define Y_AXIS_STEPS_PER_UNIT 19 // 200*8 = 1600 steps/rev = 1600/42teeth*2mm = 19.047, using 19 is an error of 1mm every 40cm not too much! and we use int operations... 37 | 38 | // Absolute Min and Max robot positions in mm (measured from center of robot pusher) 39 | #define ROBOT_MIN_X 100 40 | #define ROBOT_MIN_Y 80 41 | #define ROBOT_MAX_X 500 42 | #define ROBOT_MAX_Y 400 43 | 44 | // This is the center of the table. All units in milimeters 45 | #define ROBOT_CENTER_X 300 // Center of robot. The table is 600x1000mm, so center is 300,500 46 | #define ROBOT_CENTER_Y 500 47 | 48 | // Initial robot position in mm 49 | // The robot must be at this position at start time 50 | // Default: Centered in X and minimun position in Y 51 | #define ROBOT_INITIAL_POSITION_X 300 52 | #define ROBOT_INITIAL_POSITION_Y 45 // Measured from center of the robot pusher to the table border 53 | 54 | // Robot defense and attack lines 55 | #define ROBOT_DEFENSE_POSITION 95 56 | #define ROBOT_DEFENSE_ATTACK_POSITION 220 57 | 58 | #define POSITION_TOLERANCE 5 // 5 steps 59 | 60 | // PS3 Camera pixels 61 | // NOTE: We are using the camera at 320x240 but we are outputing a 640x480 pixel equivalent position 62 | #define CAM_PIX_WIDTH 640 63 | #define CAM_PIX_HEIGHT 480 64 | #define CAM_PIX_CENTER_X 320 65 | #define CAM_PIX_CENTER_Y 240 66 | 67 | // IMPORTANT: THIS VALUE IS FOR A 87.5 cm CARBON TUBE!! 68 | // Camera geometric calibration 69 | // Measure two known position (in mm) calculare the pixels on camera and generate divide: mm/pix 70 | #define CAM_PIX_TO_MM 1.48 // Camera scale To calculate, you need to measure two points with known coordinates 71 | 72 | // CORRECTION OF MISSING STEPS ON MOTORS 73 | // Coment this lines if you don´t want to make the corrections 74 | #define CORRECT_MISSING_STEPS_X 1 75 | #define CORRECT_MISSING_STEPS_Y 1 76 | 77 | #define MISSING_STEPS_MAX_ERROR_X 8 78 | #define MISSING_STEPS_MAX_ERROR_Y 10 79 | #define ROBOT_POSITION_CAMERA_CORRECTION_Y -20 // Correction of the position of the camera because the camera point of view and mark position 80 | 81 | // AIR HOCKEY TABLE FANS SPEED 82 | // USE 255 for FULL SPEED (if you use 15V power supply 180 is ok) 83 | #define FAN1_SPEED 255 //180 84 | #define FAN2_SPEED 255 //180 85 | 86 | // Utils (don´t modify) 87 | #define CLR(x,y) (x&=(~(1< 4 | 5 | // Variable definitions 6 | 7 | // Log and Timer variables 8 | long loop_counter; 9 | long timer_old; 10 | long timer_packet_old; 11 | long timer_value; 12 | int debug_counter; 13 | 14 | uint32_t micros_old; 15 | 16 | // We have 2 axis => 2 motor controls 0=X axis 1=Y axis (Y AXIS HAS 2 MOTORS Left and Right) 17 | int16_t speed_m[2]; // Actual speed of motors 18 | uint8_t dir_m[2]; // Actual direction of steppers motors 19 | 20 | uint16_t counter_m[2]; // counters for periods 21 | uint16_t period_m[2][8]; // Eight subperiods 22 | uint8_t period_m_index[2]; // index for subperiods 23 | 24 | // kinematic variables 25 | // position, speed and acceleration are in step units 26 | volatile int16_t position_x; // This variables are modified inside the Timer interrupts 27 | volatile int16_t position_y; 28 | 29 | int16_t speed_x; 30 | int16_t speed_y; 31 | int8_t dir_x; //(dir=1 positive, dir=-1 negative) 32 | int8_t dir_y; 33 | int16_t target_position_x; 34 | int16_t target_position_y; 35 | int16_t target_speed_x; 36 | int16_t target_speed_y; 37 | int16_t max_acceleration_x = MAX_ACCEL_X; // default maximun acceleration 38 | int16_t max_acceleration_y = MAX_ACCEL_Y; 39 | int16_t acceleration_x = MAX_ACCEL_X; 40 | int16_t acceleration_y = MAX_ACCEL_Y; 41 | int16_t accel_ramp = ACCEL_RAMP_MIN; 42 | 43 | int16_t pos_stop_x; 44 | int16_t pos_stop_y; 45 | 46 | // Trajectory prediction 47 | // Puck variables 48 | int puckPixX; 49 | int puckPixY; 50 | int puckSize; 51 | int puckCoordX; 52 | int puckCoordY; 53 | int puckOldCoordX; 54 | int puckOldCoordY; 55 | int puckSpeedX; 56 | int puckSpeedY; 57 | int puckOldSpeedX; 58 | int puckOldSpeedY; 59 | int puckSpeedXAverage; 60 | int puckSpeedYAverage; 61 | 62 | 63 | // Robot variables 64 | uint8_t robot_status; // Robot modes: 0 Init 1 Defense 2 Defense+Atack 3 Atack 65 | int robotPixX; 66 | int robotPixY; 67 | int robotCoordX; 68 | int robotCoordY; 69 | int robotCoordSamples=0; 70 | int robotCoordXAverage=0; 71 | int robotCoordYAverage=0; 72 | int robotMissingStepsErrorX; 73 | int robotMissingStepsErrorY; 74 | int defense_position; 75 | int attack_position; 76 | long attack_time; 77 | int attack_pos_x; 78 | int attack_pos_y; 79 | int attack_predict_x; 80 | int attack_predict_y; 81 | uint8_t attack_status; 82 | int8_t predict_status; // 1 Puck is going to goal 83 | int8_t predict_bounce; // 1 if puck came from a bounce 84 | int8_t predict_bounce_status; 85 | int predict_x; // X position at impact (mm) 86 | int predict_y; 87 | int predict_x_old; 88 | int predict_y_old; 89 | int predict_time; // time to impact in ms 90 | int predict_x_attack; 91 | int predict_time_attack; 92 | char tempStr2[80]; // for debug porpouses 93 | 94 | // Camera variables 95 | int cam_center_x; 96 | int cam_center_y; 97 | uint16_t cam_timestamp; 98 | float cam_pix_to_mm; 99 | float cam_rotation; //Camera rotation in radians 100 | 101 | 102 | // Serial port variables 103 | char SBuffer[14]; 104 | uint8_t readStatus; 105 | uint8_t readCounter; 106 | uint8_t newPacket; 107 | uint16_t com_pos_x; 108 | uint16_t com_pos_y; 109 | uint16_t com_speed_x; 110 | uint16_t com_speed_y; 111 | uint16_t target_x_mm; 112 | uint16_t target_y_mm; 113 | int16_t user_speed_x; 114 | int16_t user_speed_y; 115 | int16_t filt_user_speed_x; 116 | int16_t filt_user_speed_y; 117 | 118 | 119 | // Some util functions... 120 | int freeRam () { 121 | extern int __heap_start, *__brkval; 122 | int v; 123 | return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 124 | } 125 | 126 | // Arduino abs function sometimes fail! 127 | int16_t myAbs(int16_t param) 128 | { 129 | if (param<0) 130 | return -param; 131 | else 132 | return param; 133 | } 134 | 135 | // Extract sign of a variable 136 | int sign(int val) 137 | { 138 | if (val<0) 139 | return(-1); 140 | else 141 | return(1); 142 | } 143 | 144 | -------------------------------------------------------------------------------- /Arduino/AHRobot/Robot.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // Each time a new data packet from camera is reveived this function is called 4 | void newDataStrategy() 5 | { 6 | // predict_status == 1 => Puck is moving to our field directly 7 | // predict_status == 2 => Puck is moving to our field with a bounce 8 | // predict_status == 3 => Puck is in our field moving slowly, attack? 9 | 10 | // Default 11 | robot_status = 0; // Going to initial position (defense) 12 | 13 | if ((predict_status==1)&&(predict_time<350)) 14 | { 15 | // WE come from a bounce? 16 | if (predict_bounce == 1) 17 | { 18 | if ((puckSpeedYAverage)>-150) 19 | // puck is moving slowly... 20 | robot_status = 2; // Defense+Attack 21 | else 22 | { 23 | if (predict_x < 200) 24 | predict_x = 200; 25 | if (predict_x > 400) 26 | predict_x = 400; 27 | robot_status = 4; 28 | } 29 | } 30 | else 31 | { 32 | if ((predict_x > (ROBOT_MIN_X+35))&&(predict_x < (ROBOT_MAX_X-35))) 33 | robot_status = 2; //2 //Defense+attack mode 34 | else 35 | robot_status = 1; //1 36 | } 37 | } 38 | 39 | // Prediction with side bound 40 | if ((predict_status==2)&&(predict_time<350)) 41 | { 42 | robot_status = 1; //1 // Defense mode 43 | // We limit the movement in this phase 44 | if (predict_x < 200) 45 | predict_x = 200; 46 | if (predict_x > 400) 47 | predict_x = 400; 48 | } 49 | 50 | // If the puck is moving slowly in the robot field we could start an attack 51 | if ((predict_status==0)&&(puckCoordY < ROBOT_CENTER_Y)&&(myAbs(puckSpeedY)<50)) 52 | robot_status = 3; //3 53 | 54 | } 55 | 56 | void robotStrategy() 57 | { 58 | switch(robot_status) { 59 | case 0: 60 | // Go to defense position 61 | com_pos_y = defense_position; 62 | com_pos_x = ROBOT_CENTER_X; //center X axis 63 | setSpeedS(com_speed_x,com_speed_y); 64 | setPosition(com_pos_x,com_pos_y); 65 | attack_time = 0; 66 | break; 67 | case 1: 68 | // Defense mode (only move on X axis on the defense line) 69 | com_pos_y = defense_position; 70 | com_pos_x = predict_x; 71 | setSpeedS(com_speed_x,com_speed_y); 72 | setPosition(com_pos_x,com_pos_y); 73 | attack_time = 0; 74 | break; 75 | case 2: 76 | // Defense+attack 77 | if (predict_time_attack<180) // If time is less than 180ms we start the attack 78 | { 79 | com_pos_y = attack_position + 80; 80 | com_pos_x = predict_x_attack; 81 | setSpeedS(com_speed_x,com_speed_y); 82 | setPosition(com_pos_x,com_pos_y); // We use a straight line path 83 | } 84 | else // Defense position 85 | { 86 | com_pos_y = predict_y; 87 | com_pos_x = predict_x; // predict_x_attack; 88 | setSpeedS(com_speed_x,com_speed_y); 89 | setPosition(com_pos_x,com_pos_y); 90 | attack_time = 0; 91 | } 92 | 93 | break; 94 | case 3: 95 | // ATTACK MODE 96 | if (attack_time == 0) 97 | { 98 | attack_predict_x = predictPuckXPosition(500); 99 | attack_predict_y = predictPuckYPosition(500); 100 | if ((attack_predict_x > 120)&&(attack_predict_x < 480)&&(attack_predict_y >180)&&(attack_predict_y<320)) 101 | { 102 | attack_time = millis() + 500; // Prepare an attack in 500ms 103 | attack_pos_x = attack_predict_x; // predict_x 104 | attack_pos_y = attack_predict_y; // predict_y 105 | Serial.print("AM:"); 106 | //Serial.print(attack_time); 107 | //Serial.print(","); 108 | Serial.print(attack_pos_x); 109 | Serial.print(","); 110 | Serial.println(attack_pos_y); 111 | //Serial.print(" "); 112 | // Go to pre-attack position 113 | com_pos_x = attack_pos_x; 114 | com_pos_y = attack_pos_y-120; 115 | setSpeedS(com_speed_x/2,com_speed_y/2); 116 | setPosition(com_pos_x,com_pos_y); 117 | attack_status = 1; 118 | } 119 | else 120 | { 121 | attack_time = 0; // Continue... 122 | attack_status = 0; 123 | // And go to defense position 124 | com_pos_y = defense_position; 125 | com_pos_x = ROBOT_CENTER_X; //center X axis 126 | setSpeedS(com_speed_x/2,com_speed_y/2); 127 | setPosition(com_pos_x,com_pos_y); 128 | } 129 | } 130 | else 131 | { 132 | if (attack_status == 1) 133 | { 134 | if ((attack_time-millis())<200) 135 | { 136 | // Attack movement 137 | com_pos_x = predictPuckXPosition(200); 138 | com_pos_y = predictPuckYPosition(200) + 80; 139 | setSpeedS(com_speed_x,com_speed_y); 140 | setPosition(com_pos_x,com_pos_y); 141 | 142 | Serial.print("ATTACK:"); 143 | Serial.print(com_pos_x); 144 | Serial.print(","); 145 | Serial.println(com_pos_y-80); 146 | 147 | attack_status = 2; // Attacking 148 | } 149 | else // attack_status=1 but it´s no time to attack yet 150 | { 151 | // Go to pre-attack position 152 | com_pos_x = attack_pos_x; 153 | com_pos_y = attack_pos_y-120; 154 | setSpeedS(com_speed_x/2,com_speed_y/2); 155 | setPosition(com_pos_x,com_pos_y); 156 | } 157 | } 158 | if (attack_status==2) 159 | { 160 | if (millis()>(attack_time+80)) // Attack move is done? => Reset to defense position 161 | { 162 | Serial.print("RESET"); 163 | attack_time = 0; 164 | robot_status = 0; 165 | attack_status = 0; 166 | } 167 | } 168 | } 169 | break; 170 | case 4: 171 | // The puck came from a bounce 172 | // Only defense now (we could improve this in future) 173 | // Defense mode (only move on X axis on the defense line) 174 | com_pos_y = defense_position; 175 | com_pos_x = predict_x; 176 | setSpeedS(com_speed_x,com_speed_y); 177 | setPosition(com_pos_x,com_pos_y); 178 | attack_time = 0; 179 | break; 180 | default: 181 | // Default : go to defense position 182 | com_pos_y = defense_position; 183 | com_pos_x = ROBOT_CENTER_X; // center 184 | setSpeedS(com_speed_x,com_speed_y); 185 | setPosition(com_pos_x,com_pos_y); 186 | attack_time = 0; 187 | } 188 | } 189 | -------------------------------------------------------------------------------- /Arduino/AHRobot/Steppers.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // STEPPERS MOTOR CONTROL 4 | // Updated: Now it supports DRV8825 drivers 5 | // SPEED, ACCELERATION AND POSITION CONTROL using Arduino 16 bit Timer interrupts 6 | 7 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 8 | // X MOTOR 9 | // X-STEP: A0 (PF0) 10 | // X-DIR: A1 (PF1) 11 | // X-ENABLE: D38 (PD7) 12 | // Y MOTOR (Y-LEFT) 13 | // Y-STEP: A6 (PF6) 14 | // Y-DIR: A7 (PF7) 15 | // Y-ENABLE: A2 (PF2) 16 | // Z MOTOR (Y-RIGHT) 17 | // Z-STEP: D46 (PL3) 18 | // Z-DIR: D48 (PL1) 19 | // Z-ENABLE: A8 (PK0) 20 | // Y and Z motors controls the Y axis of the robot (same output) 21 | 22 | // We control the speed of the motors with interrupts (Timer1 and Timer3) tested up to 25Khz. 23 | // The position of the motor is controlled at 1Khz (in the main loop) 24 | 25 | 26 | // TIMER 1 : STEPPER MOTOR SPEED CONTROL X-AXIS 27 | ISR(TIMER1_COMPA_vect) 28 | { 29 | if (dir_x==0) 30 | return; 31 | 32 | SET(PORTF,0); // STEP X-AXIS 33 | position_x += dir_x; 34 | __asm__ __volatile__ ( 35 | "nop" "\n\t" 36 | "nop" "\n\t" 37 | "nop" "\n\t" 38 | "nop" "\n\t" 39 | "nop" "\n\t" 40 | "nop" "\n\t" 41 | "nop" "\n\t" 42 | "nop" "\n\t" 43 | "nop" "\n\t" 44 | "nop" "\n\t" 45 | "nop" "\n\t" 46 | "nop" "\n\t" 47 | "nop" "\n\t" 48 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 49 | CLR(PORTF,0); 50 | } 51 | 52 | // TIMER 3 : STEPPER MOTOR SPEED CONTROL Y-AXIS (2 Motors, left and right) 53 | ISR(TIMER3_COMPA_vect) 54 | { 55 | if (dir_y==0) 56 | return; 57 | 58 | SET(PORTF,6); // STEP Y-AXIS (Y-left) 59 | SET(PORTL,3); // STEP Z-AXIS (Y-right) 60 | position_y += dir_y; 61 | __asm__ __volatile__ ( 62 | "nop" "\n\t" 63 | "nop" "\n\t" 64 | "nop" "\n\t" 65 | "nop" "\n\t" 66 | "nop" "\n\t" 67 | "nop" "\n\t" 68 | "nop" "\n\t" 69 | "nop" "\n\t" 70 | "nop" "\n\t" 71 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 72 | CLR(PORTF,6); 73 | CLR(PORTL,3); 74 | } 75 | 76 | // We use a ramp for acceleration and deceleration 77 | // To calculate the point we should start to decelerate we use this formula: 78 | // stop_position = actual_posicion + (actual_speed*actual_speed)/(2*max_deceleration) 79 | // Beacuase we are using a S-profile we apply a correction factor to the above formula (0.85) 80 | // Input parameters: 81 | // target_position_x 82 | // target_speed_x 83 | // max_acceleration_x 84 | 85 | void positionControl() 86 | { 87 | //int16_t pos_stop; 88 | int32_t temp; 89 | uint32_t timer; 90 | int16_t dt; 91 | 92 | SET(PORTF,3); // for external timing debug 93 | timer = micros(); 94 | // dt = delta time in microseconds... 95 | dt = constrain(timer-micros_old,0,2000); // Limit dt (it should be around 1000 most times) 96 | //Serial.println(dt); 97 | micros_old = timer; 98 | 99 | // We use an acceleration ramp to imitate an S-curve profile at the begining and end (depend on speed) 100 | acceleration_x = map(abs(speed_x),0,accel_ramp,MIN_ACCEL_X,max_acceleration_x); 101 | acceleration_x = constrain(acceleration_x,MIN_ACCEL_X,max_acceleration_x); 102 | 103 | acceleration_y = map(abs(speed_y),0,accel_ramp,MIN_ACCEL_Y,max_acceleration_y); 104 | acceleration_y = constrain(acceleration_y,MIN_ACCEL_Y,max_acceleration_y); 105 | 106 | // X AXIS 107 | temp = (long)speed_x*speed_x; 108 | temp = temp/(1800*(long)acceleration_x); // 2000*0.85 = 1700 0.85 is a compensation for deceleration ramp (S-curve) 109 | pos_stop_x = position_x + sign(speed_x)*temp; 110 | if (target_position_x>position_x) // Positive move 111 | { 112 | if (pos_stop_x >= target_position_x) // Start decelerating? 113 | setMotorXSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 114 | else 115 | setMotorXSpeed(target_speed_x,dt); // The aceleration ramp is done inside the setSpeed function 116 | } 117 | else // Negative move 118 | { 119 | if (pos_stop_x <= target_position_x) // Start decelerating? 120 | setMotorXSpeed(0,dt); 121 | else 122 | setMotorXSpeed(-target_speed_x,dt); 123 | } 124 | 125 | // Y AXIS 126 | temp = (long)speed_y*speed_y; 127 | temp = temp/(1800*(long)acceleration_y); 128 | pos_stop_y = position_y + sign(speed_y)*temp; 129 | if (target_position_y>position_y) // Positive move 130 | { 131 | if (pos_stop_y >= target_position_y) // Start decelerating? 132 | setMotorYSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 133 | else 134 | setMotorYSpeed(target_speed_y,dt); // The aceleration ramp is done inside the setSpeed function 135 | } 136 | else // Negative move 137 | { 138 | if (pos_stop_y <= target_position_y) // Start decelerating? 139 | setMotorYSpeed(0,dt); 140 | else 141 | setMotorYSpeed(-target_speed_y,dt); 142 | } 143 | CLR(PORTF,3); // for external timing debug 144 | } 145 | 146 | // Speed could be positive or negative 147 | void setMotorXSpeed(int16_t tspeed, int16_t dt) 148 | { 149 | long timer_period; 150 | int16_t accel; 151 | 152 | // Limit max speed 153 | if (tspeed > MAX_SPEED_X) 154 | tspeed = MAX_SPEED_X; 155 | else if (tspeed < -MAX_SPEED_X) 156 | tspeed = -MAX_SPEED_X; 157 | //Serial.println(tspeed); 158 | 159 | // We limit acceleration => speed ramp 160 | accel = ((long)acceleration_x*dt)/1000; // We divide by 1000 because dt are in microseconds 161 | if (((long)tspeed-speed_x)>accel) // We use long here to avoid overflow on the operation 162 | speed_x += accel; 163 | else if (((long)speed_x-tspeed)>accel) 164 | speed_x -= accel; 165 | else 166 | speed_x = tspeed; 167 | 168 | // Check if we need to change the direction pins 169 | if ((speed_x==0)&&(dir_x!=0)) 170 | dir_x = 0; 171 | else if ((speed_x>0)&&(dir_x!=1)) 172 | { 173 | #ifdef INVERT_X_AXIS 174 | CLR(PORTF,1); // X-DIR 175 | #else 176 | SET(PORTF,1); 177 | #endif 178 | dir_x = 1; 179 | } 180 | else if ((speed_x<0)&&(dir_x!=-1)) 181 | { 182 | #ifdef INVERT_X_AXIS 183 | SET(PORTF,1); 184 | #else 185 | CLR(PORTF,1); 186 | #endif 187 | dir_x = -1; 188 | } 189 | 190 | if (speed_x==0) 191 | timer_period = ZERO_SPEED; 192 | else if (speed_x>0) 193 | timer_period = 2000000/speed_x; // 2Mhz timer 194 | else 195 | timer_period = 2000000/-speed_x; 196 | 197 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 198 | timer_period = ZERO_SPEED; 199 | 200 | OCR1A = timer_period; 201 | // Check if we need to reset the timer... 202 | if (TCNT1 > OCR1A) 203 | TCNT1 = 0; 204 | } 205 | 206 | // Speed could be positive or negative 207 | void setMotorYSpeed(int16_t tspeed,int16_t dt) 208 | { 209 | long timer_period; 210 | int16_t accel; 211 | 212 | // Limit max speed 213 | if (tspeed > MAX_SPEED_Y) 214 | tspeed = MAX_SPEED_Y; 215 | else if (tspeed < -MAX_SPEED_Y) 216 | tspeed = -MAX_SPEED_Y; 217 | //Serial.println(tspeed); 218 | 219 | // We limit acceleration => speed ramp 220 | accel = ((long)acceleration_y*dt)/1000; 221 | if (((long)tspeed-speed_y)>accel) 222 | speed_y += accel; 223 | else if (((long)speed_y-tspeed)>accel) 224 | speed_y -= accel; 225 | else 226 | speed_y = tspeed; 227 | 228 | // Check if we need to change the direction pins 229 | if ((speed_y==0)&&(dir_y!=0)) 230 | dir_y = 0; 231 | else if ((speed_y>0)&&(dir_y!=1)) 232 | { 233 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 234 | CLR(PORTF,7); 235 | #else 236 | SET(PORTF,7); 237 | #endif 238 | 239 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 240 | CLR(PORTL,1); 241 | #else 242 | SET(PORTL,1); 243 | #endif 244 | 245 | dir_y = 1; 246 | } 247 | else if ((speed_y<0)&&(dir_y!=-1)) 248 | { 249 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 250 | SET(PORTF,7); 251 | #else 252 | CLR(PORTF,7); 253 | #endif 254 | 255 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 256 | SET(PORTL,1); 257 | #else 258 | CLR(PORTL,1); 259 | #endif 260 | 261 | dir_y = -1; 262 | } 263 | 264 | if (speed_y==0) 265 | timer_period = ZERO_SPEED; 266 | else if (speed_y>0) 267 | timer_period = 2000000/speed_y; // 2Mhz timer 268 | else 269 | timer_period = 2000000/-speed_y; 270 | 271 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 272 | timer_period = ZERO_SPEED; 273 | 274 | OCR3A = timer_period; 275 | // Check if we need to reset the timer... 276 | if (TCNT3 > OCR3A) 277 | TCNT3 = 0; 278 | } 279 | 280 | // Set speed on each axis in steps/sec 281 | void setSpeedS(int target_sx, int target_sy) 282 | { 283 | target_sx = constrain(target_sx,0,MAX_SPEED_X); 284 | target_sy = constrain(target_sy,0,MAX_SPEED_Y); 285 | target_speed_x = target_sx; 286 | target_speed_y = target_sy; 287 | } 288 | 289 | // set Robot position in mm. 290 | // This function check for valid robot positions values 291 | // Convert from mm units to steps 292 | void setPosition(int target_x_mm_new, int target_y_mm_new) 293 | { 294 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 295 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 296 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 297 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 298 | } 299 | 300 | // set Robot position in mm. 301 | // This function check for valid robot positions values 302 | // Convert from mm units to steps 303 | void setPosition_straight(int target_x_mm_new, int target_y_mm_new) 304 | { 305 | int old_target_position_x; 306 | int old_target_position_y; 307 | int diff_x; 308 | int diff_y; 309 | 310 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 311 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 312 | old_target_position_x = target_position_x; 313 | old_target_position_y = target_position_y; 314 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 315 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 316 | // Speed adjust to draw straight lines 317 | diff_x = myAbs(target_position_x - old_target_position_x); 318 | diff_y = myAbs(target_position_y - old_target_position_y); 319 | if (diff_x > diff_y) // Wich axis should be slower? 320 | { 321 | com_speed_x = target_speed_x; 322 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 323 | setSpeedS(com_speed_x,com_speed_y); 324 | } 325 | else 326 | { 327 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 328 | com_speed_y = target_speed_y; 329 | setSpeedS(com_speed_x,com_speed_y); 330 | } 331 | } 332 | 333 | // set Robot position in 1/10 mm. 334 | // This function check for valid robot positions values 335 | // Convert from 1/10 mm units to steps 336 | // This function moves the robot in a straight line 337 | void setPosition_mm10_straight(int target_x_mm_new, int target_y_mm_new) 338 | { 339 | int old_target_position_x; 340 | int old_target_position_y; 341 | int diff_x; 342 | int diff_y; 343 | 344 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X*10,ROBOT_MAX_X*10); 345 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y*10,ROBOT_MAX_Y*10); 346 | old_target_position_x = target_position_x; 347 | old_target_position_y = target_position_y; 348 | target_position_x = (float)target_x_mm*X_AXIS_STEPS_PER_UNIT/10.0; 349 | target_position_y = (float)target_y_mm*Y_AXIS_STEPS_PER_UNIT/10.0; 350 | // Speed adjust to draw straight lines 351 | diff_x = myAbs(target_position_x - old_target_position_x); 352 | diff_y = myAbs(target_position_y - old_target_position_y); 353 | if (diff_x > diff_y) // Wich axis should be slower? 354 | { 355 | com_speed_x = target_speed_x; 356 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 357 | setSpeedS(com_speed_x,com_speed_y); 358 | } 359 | else 360 | { 361 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 362 | com_speed_y = target_speed_y; 363 | setSpeedS(com_speed_x,com_speed_y); 364 | } 365 | } 366 | 367 | 368 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test/AHR_Motor_Test.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // MOTOR TEST! 4 | // If you change some parameter on Configuration.h 5 | // remember to copy it to the final project AHRobot. 6 | 7 | // Author: Jose Julio (@jjdrones) 8 | // Hardware: Arduino MEGA + Ramps 1.4 9 | // Date: 21/01/2014 10 | // Updated: 11 | // Version: 1.01 12 | // Project page : Spanish: English: 13 | // GIT repository: 14 | // License: Open Software GPL License 15 | 16 | // ROBOT and USER configuration parameters 17 | #include "Configuration.h" 18 | #include "Definitions.h" // Variable definitions 19 | 20 | 21 | void print_values() 22 | { 23 | //Serial.print(com_pos_x); 24 | //Serial.print(","); 25 | //Serial.print(com_pos_y); 26 | //Serial.print(","); 27 | //Serial.print(position_x); 28 | //Serial.print(","); 29 | //Serial.println(position_y); 30 | 31 | } 32 | 33 | void setup() 34 | { 35 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 36 | // X MOTOR 37 | // X-STEP: A0 (PF0) 38 | // X-DIR: A1 (PF1) 39 | // X-ENABLE: D38 (PD7) 40 | // Y MOTOR (Y-LEFT) 41 | // Y-STEP: A6 (PF6) 42 | // Y-DIR: A7 (PF7) 43 | // Y-ENABLE: A2 (PF2) 44 | // Z MOTOR (Y-RIGHT) 45 | // Z-STEP: D46 (PL3) 46 | // Z-DIR: D48 (PL1) 47 | // Z-ENABLE: A8 (PK0) 48 | 49 | // STEPPER PINS 50 | // X_AXIS 51 | pinMode(38,OUTPUT); // ENABLE MOTOR 52 | pinMode(A0,OUTPUT); // STEP MOTOR 53 | pinMode(A1,OUTPUT); // DIR MOTOR 54 | // Y_AXIS (Y-LEFT) 55 | pinMode(A2,OUTPUT); // ENABLE MOTOR 56 | pinMode(A6,OUTPUT); // STEP MOTOR 57 | pinMode(A7,OUTPUT); // DIR MOTOR 58 | // Z_AXIS (Y-RIGHT) 59 | pinMode(A8,OUTPUT); // ENABLE MOTOR 60 | pinMode(46,OUTPUT); // STEP MOTOR 61 | pinMode(48,OUTPUT); // DIR MOTOR 62 | 63 | pinMode(A3,OUTPUT); // DEBUG PIN FOR OSCILLOSCOPE TIME MEASURES 64 | 65 | pinMode(19,INPUT); // RX1 Serial Port 1 66 | pinMode(18,OUTPUT); // TX1 67 | 68 | //FANS and LEDS 69 | pinMode(8,OUTPUT); 70 | pinMode(9,OUTPUT); 71 | pinMode(10,OUTPUT); 72 | pinMode(13,OUTPUT); 73 | 74 | // Disable Motors 75 | digitalWrite(38,HIGH); 76 | digitalWrite(A2,HIGH); 77 | digitalWrite(A8,HIGH); 78 | 79 | Serial.begin(115200); 80 | Serial.println("AHR Robot Motor test v1.01"); 81 | Serial.println("Initializing robot..."); 82 | Serial.print("Free Memory: "); 83 | Serial.print(freeRam()); 84 | Serial.println(); 85 | delay(2500); 86 | 87 | //LED blink 88 | for (uint8_t k=0;k<4;k++) 89 | { 90 | digitalWrite(13,HIGH); 91 | delay(300); 92 | digitalWrite(13,LOW); 93 | delay(300); 94 | } 95 | 96 | // We use TIMER 1 for stepper motor X AXIS and Timer 3 for Y AXIS 97 | // STEPPER MOTORS INITIALIZATION 98 | // TIMER1 CTC MODE 99 | TCCR1B &= ~(1<=1000) // 1Khz loop 191 | { 192 | //dt = (timer_value-timer_old)/1000; 193 | timer_old = timer_value; 194 | loop_counter++; 195 | 196 | if (loop_counter == 3000) 197 | print_values(); 198 | 199 | if (loop_counter == 5000) 200 | { 201 | Serial.println("Moving the robot 5cm in X"); 202 | print_values(); 203 | // We move the robot 5cm in X 204 | com_pos_x += 50; 205 | setPosition(com_pos_x,com_pos_y); 206 | print_values(); 207 | } 208 | 209 | if (loop_counter == 15000) 210 | { 211 | Serial.println("Moving the robot 5cm in Y"); 212 | // We move the robot 5cm in X 213 | print_values(); 214 | 215 | com_pos_y += 50; 216 | setPosition(com_pos_x,com_pos_y); 217 | print_values(); 218 | } 219 | 220 | positionControl(); 221 | } // 1Khz loop 222 | } 223 | 224 | 225 | 226 | 227 | 228 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test/Configuration.h: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // USER CONFIGURATIONS HERE 4 | // ROBOT DIMENSIONS, MAX SPEED, MAX ACCELERATION, CALIBRATION 5 | 6 | // THIS VALUES DEPENDS ON THE VOLTAGE, MOTORS, PULLEYS AND ROBOT CONSTRUCTION 7 | // RECOMMENDED VALUES FOR 12V POWER SUPPLY 8 | #define MIN_ACCEL_X 60 9 | #define MAX_ACCEL_X 300 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 10 | #define MIN_ACCEL_Y 60 11 | #define MAX_ACCEL_Y 145 //140//220 12 | #define MAX_SPEED_X 25000 //max 25000 for 12V // Maximun speed in steps/seg 13 | #define MAX_SPEED_Y 25000 14 | 15 | // RECOMMENDED VALUES FOR 15V POWER SUPPLY 16 | //#define MIN_ACCEL_X 40 17 | //#define MAX_ACCEL_X 380 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 18 | //#define MIN_ACCEL_Y 70 19 | //#define MAX_ACCEL_Y 200 //140//220 20 | //#define MAX_SPEED_X 28000 //max 25000 for 12V // Maximun speed in steps/seg 21 | //#define MAX_SPEED_Y 28000 22 | 23 | 24 | // This is for the Accel ramp implementation (to smooth the intial acceleration), simplified S-profile 25 | #define ACCEL_RAMP_MIN 2500 // The S profile is generated up to this speed 26 | #define ACCEL_RAMP_MAX 10000 27 | 28 | // UNCOMMENT THIS LINES TO INVERT MOTORS 29 | #define INVERT_X_AXIS 1 30 | //#define INVERT_Y_AXIS 1 //Y-LEFT 31 | //#define INVERT_Z_AXIS 1 //Y_RIGHT 32 | 33 | // Geometric calibration. 34 | // This depends on the pulley teeth. For 42 teeth GT2 => 19, for 40 teeth GT2 => 20, for 16 teeth T5 => 20 35 | #define X_AXIS_STEPS_PER_UNIT 19 // With 42 teeth GT2 pulley and 1/8 microstepping on drivers 36 | #define Y_AXIS_STEPS_PER_UNIT 19 // 200*8 = 1600 steps/rev = 1600/42teeth*2mm = 19.047, using 19 is an error of 1mm every 40cm not too much! and we use int operations... 37 | 38 | // Absolute Min and Max robot positions in mm (measured from center of robot pusher) 39 | #define ROBOT_MIN_X 100 40 | #define ROBOT_MIN_Y 80 41 | #define ROBOT_MAX_X 500 42 | #define ROBOT_MAX_Y 400 43 | 44 | // This is the center of the table. All units in milimeters 45 | #define ROBOT_CENTER_X 300 // Center of robot. The table is 600x1000mm, so center is 300,500 46 | #define ROBOT_CENTER_Y 500 47 | 48 | // Initial robot position in mm 49 | // The robot must be at this position at start time 50 | // Default: Centered in X and minimun position in Y 51 | #define ROBOT_INITIAL_POSITION_X 300 52 | #define ROBOT_INITIAL_POSITION_Y 45 // Measured from center of the robot pusher to the table border 53 | 54 | // Robot defense and attack lines 55 | #define ROBOT_DEFENSE_POSITION 95 56 | #define ROBOT_DEFENSE_ATTACK_POSITION 220 57 | 58 | #define POSITION_TOLERANCE 5 // 5 steps 59 | 60 | // PS3 Camera pixels 61 | // NOTE: We are using the camera at 320x240 but we are outputing a 640x480 pixel equivalent position 62 | #define CAM_PIX_WIDTH 640 63 | #define CAM_PIX_HEIGHT 480 64 | #define CAM_PIX_CENTER_X 320 65 | #define CAM_PIX_CENTER_Y 240 66 | 67 | // IMPORTANT: THIS VALUE IS FOR A 87.5 cm CARBON TUBE!! 68 | // Camera geometric calibration 69 | // Measure two known position (in mm) calculare the pixels on camera and generate divide: mm/pix 70 | #define CAM_PIX_TO_MM 1.48 // Camera scale To calculate, you need to measure two points with known coordinates 71 | 72 | // CORRECTION OF MISSING STEPS ON MOTORS 73 | // Coment this lines if you don´t want to make the corrections 74 | #define CORRECT_MISSING_STEPS_X 1 75 | #define CORRECT_MISSING_STEPS_Y 1 76 | 77 | #define MISSING_STEPS_MAX_ERROR_X 8 78 | #define MISSING_STEPS_MAX_ERROR_Y 10 79 | #define ROBOT_POSITION_CAMERA_CORRECTION_Y -20 // Correction of the position of the camera because the camera point of view and mark position 80 | 81 | // AIR HOCKEY TABLE FANS SPEED 82 | // USE 255 for FULL SPEED (if you use 15V power supply 180 is ok) 83 | #define FAN1_SPEED 255 //180 84 | #define FAN2_SPEED 255 //180 85 | 86 | // Utils (don´t modify) 87 | #define CLR(x,y) (x&=(~(1< 4 | 5 | // Variable definitions 6 | 7 | // Log and Timer variables 8 | long loop_counter; 9 | long timer_old; 10 | long timer_packet_old; 11 | long timer_value; 12 | int debug_counter; 13 | 14 | uint32_t micros_old; 15 | 16 | // We have 2 axis => 2 motor controls 0=X axis 1=Y axis (Y AXIS HAS 2 MOTORS Left and Right) 17 | int16_t speed_m[2]; // Actual speed of motors 18 | uint8_t dir_m[2]; // Actual direction of steppers motors 19 | 20 | uint16_t counter_m[2]; // counters for periods 21 | uint16_t period_m[2][8]; // Eight subperiods 22 | uint8_t period_m_index[2]; // index for subperiods 23 | 24 | // kinematic variables 25 | // position, speed and acceleration are in step units 26 | volatile int16_t position_x; // This variables are modified inside the Timer interrupts 27 | volatile int16_t position_y; 28 | 29 | int16_t speed_x; 30 | int16_t speed_y; 31 | int16_t max_speed_x; 32 | int16_t max_speed_y; 33 | 34 | int8_t dir_x; //(dir=1 positive, dir=-1 negative) 35 | int8_t dir_y; 36 | int16_t target_position_x; 37 | int16_t target_position_y; 38 | int16_t target_speed_x; 39 | int16_t target_speed_y; 40 | int16_t max_acceleration_x = MAX_ACCEL_X; // default maximun acceleration 41 | int16_t max_acceleration_y = MAX_ACCEL_Y; 42 | int16_t acceleration_x = MAX_ACCEL_X; 43 | int16_t acceleration_y = MAX_ACCEL_Y; 44 | int16_t accel_ramp = ACCEL_RAMP_MIN; 45 | 46 | int16_t pos_stop_x; 47 | int16_t pos_stop_y; 48 | 49 | uint16_t com_pos_x; 50 | uint16_t com_pos_y; 51 | uint16_t com_speed_x; 52 | uint16_t com_speed_y; 53 | uint16_t target_x_mm; 54 | uint16_t target_y_mm; 55 | int16_t user_speed_x; 56 | int16_t user_speed_y; 57 | int16_t filt_user_speed_x; 58 | int16_t filt_user_speed_y; 59 | 60 | 61 | // Some util functions... 62 | int freeRam () { 63 | extern int __heap_start, *__brkval; 64 | int v; 65 | return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 66 | } 67 | 68 | int16_t myAbs(int16_t param) 69 | { 70 | if (param<0) 71 | return -param; 72 | else 73 | return param; 74 | } 75 | 76 | int sign(int val) 77 | { 78 | if (val<0) 79 | return(-1); 80 | else 81 | return(1); 82 | } 83 | 84 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test/Steppers.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // STEPPERS MOTOR CONTROL 4 | // Updated: Now it supports DRV8825 drivers 5 | // SPEED, ACCELERATION AND POSITION CONTROL using Arduino 16 bit Timer interrupts 6 | 7 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 8 | // X MOTOR 9 | // X-STEP: A0 (PF0) 10 | // X-DIR: A1 (PF1) 11 | // X-ENABLE: D38 (PD7) 12 | // Y MOTOR (Y-LEFT) 13 | // Y-STEP: A6 (PF6) 14 | // Y-DIR: A7 (PF7) 15 | // Y-ENABLE: A2 (PF2) 16 | // Z MOTOR (Y-RIGHT) 17 | // Z-STEP: D46 (PL3) 18 | // Z-DIR: D48 (PL1) 19 | // Z-ENABLE: A8 (PK0) 20 | // Y and Z motors controls the Y axis of the robot (same output) 21 | 22 | // We control the speed of the motors with interrupts (Timer1 and Timer3) tested up to 25Khz. 23 | // The position of the motor is controlled at 1Khz (in the main loop) 24 | 25 | 26 | // TIMER 1 : STEPPER MOTOR SPEED CONTROL X-AXIS 27 | ISR(TIMER1_COMPA_vect) 28 | { 29 | if (dir_x==0) 30 | return; 31 | 32 | SET(PORTF,0); // STEP X-AXIS 33 | position_x += dir_x; 34 | __asm__ __volatile__ ( 35 | "nop" "\n\t" 36 | "nop" "\n\t" 37 | "nop" "\n\t" 38 | "nop" "\n\t" 39 | "nop" "\n\t" 40 | "nop" "\n\t" 41 | "nop" "\n\t" 42 | "nop" "\n\t" 43 | "nop" "\n\t" 44 | "nop" "\n\t" 45 | "nop" "\n\t" 46 | "nop" "\n\t" 47 | "nop" "\n\t" 48 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 49 | CLR(PORTF,0); 50 | } 51 | 52 | // TIMER 3 : STEPPER MOTOR SPEED CONTROL Y-AXIS (2 Motors, left and right) 53 | ISR(TIMER3_COMPA_vect) 54 | { 55 | if (dir_y==0) 56 | return; 57 | 58 | SET(PORTF,6); // STEP Y-AXIS (Y-left) 59 | SET(PORTL,3); // STEP Z-AXIS (Y-right) 60 | position_y += dir_y; 61 | __asm__ __volatile__ ( 62 | "nop" "\n\t" 63 | "nop" "\n\t" 64 | "nop" "\n\t" 65 | "nop" "\n\t" 66 | "nop" "\n\t" 67 | "nop" "\n\t" 68 | "nop" "\n\t" 69 | "nop" "\n\t" 70 | "nop" "\n\t" 71 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 72 | CLR(PORTF,6); 73 | CLR(PORTL,3); 74 | } 75 | 76 | // We use a ramp for acceleration and deceleration 77 | // To calculate the point we should start to decelerate we use this formula: 78 | // stop_position = actual_posicion + (actual_speed*actual_speed)/(2*max_deceleration) 79 | // Beacuase we are using a S-profile we apply a correction factor to the above formula (0.85) 80 | // Input parameters: 81 | // target_position_x 82 | // target_speed_x 83 | // max_acceleration_x 84 | 85 | void positionControl() 86 | { 87 | //int16_t pos_stop; 88 | int32_t temp; 89 | uint32_t timer; 90 | int16_t dt; 91 | 92 | SET(PORTF,3); // for external timing debug 93 | timer = micros(); 94 | // dt = delta time in microseconds... 95 | dt = constrain(timer-micros_old,0,2000); // Limit dt (it should be around 1000 most times) 96 | //Serial.println(dt); 97 | micros_old = timer; 98 | 99 | // We use an acceleration ramp to imitate an S-curve profile at the begining and end (depend on speed) 100 | acceleration_x = map(abs(speed_x),0,accel_ramp,MIN_ACCEL_X,max_acceleration_x); 101 | acceleration_x = constrain(acceleration_x,MIN_ACCEL_X,max_acceleration_x); 102 | 103 | acceleration_y = map(abs(speed_y),0,accel_ramp,MIN_ACCEL_Y,max_acceleration_y); 104 | acceleration_y = constrain(acceleration_y,MIN_ACCEL_Y,max_acceleration_y); 105 | 106 | // X AXIS 107 | temp = (long)speed_x*speed_x; 108 | temp = temp/(1800*(long)acceleration_x); // 2000*0.85 = 1700 0.85 is a compensation for deceleration ramp (S-curve) 109 | pos_stop_x = position_x + sign(speed_x)*temp; 110 | if (target_position_x>position_x) // Positive move 111 | { 112 | if (pos_stop_x >= target_position_x) // Start decelerating? 113 | setMotorXSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 114 | else 115 | setMotorXSpeed(target_speed_x,dt); // The aceleration ramp is done inside the setSpeed function 116 | } 117 | else // Negative move 118 | { 119 | if (pos_stop_x <= target_position_x) // Start decelerating? 120 | setMotorXSpeed(0,dt); 121 | else 122 | setMotorXSpeed(-target_speed_x,dt); 123 | } 124 | 125 | // Y AXIS 126 | temp = (long)speed_y*speed_y; 127 | temp = temp/(1800*(long)acceleration_y); 128 | pos_stop_y = position_y + sign(speed_y)*temp; 129 | if (target_position_y>position_y) // Positive move 130 | { 131 | if (pos_stop_y >= target_position_y) // Start decelerating? 132 | setMotorYSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 133 | else 134 | setMotorYSpeed(target_speed_y,dt); // The aceleration ramp is done inside the setSpeed function 135 | } 136 | else // Negative move 137 | { 138 | if (pos_stop_y <= target_position_y) // Start decelerating? 139 | setMotorYSpeed(0,dt); 140 | else 141 | setMotorYSpeed(-target_speed_y,dt); 142 | } 143 | CLR(PORTF,3); // for external timing debug 144 | } 145 | 146 | // Speed could be positive or negative 147 | void setMotorXSpeed(int16_t tspeed, int16_t dt) 148 | { 149 | long timer_period; 150 | int16_t accel; 151 | 152 | // Limit max speed 153 | if (tspeed > MAX_SPEED_X) 154 | tspeed = MAX_SPEED_X; 155 | else if (tspeed < -MAX_SPEED_X) 156 | tspeed = -MAX_SPEED_X; 157 | //Serial.println(tspeed); 158 | 159 | // We limit acceleration => speed ramp 160 | accel = ((long)acceleration_x*dt)/1000; // We divide by 1000 because dt are in microseconds 161 | if (((long)tspeed-speed_x)>accel) // We use long here to avoid overflow on the operation 162 | speed_x += accel; 163 | else if (((long)speed_x-tspeed)>accel) 164 | speed_x -= accel; 165 | else 166 | speed_x = tspeed; 167 | 168 | // Check if we need to change the direction pins 169 | if ((speed_x==0)&&(dir_x!=0)) 170 | dir_x = 0; 171 | else if ((speed_x>0)&&(dir_x!=1)) 172 | { 173 | #ifdef INVERT_X_AXIS 174 | CLR(PORTF,1); // X-DIR 175 | #else 176 | SET(PORTF,1); 177 | #endif 178 | dir_x = 1; 179 | } 180 | else if ((speed_x<0)&&(dir_x!=-1)) 181 | { 182 | #ifdef INVERT_X_AXIS 183 | SET(PORTF,1); 184 | #else 185 | CLR(PORTF,1); 186 | #endif 187 | dir_x = -1; 188 | } 189 | 190 | if (speed_x==0) 191 | timer_period = ZERO_SPEED; 192 | else if (speed_x>0) 193 | timer_period = 2000000/speed_x; // 2Mhz timer 194 | else 195 | timer_period = 2000000/-speed_x; 196 | 197 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 198 | timer_period = ZERO_SPEED; 199 | 200 | OCR1A = timer_period; 201 | // Check if we need to reset the timer... 202 | if (TCNT1 > OCR1A) 203 | TCNT1 = 0; 204 | } 205 | 206 | // Speed could be positive or negative 207 | void setMotorYSpeed(int16_t tspeed,int16_t dt) 208 | { 209 | long timer_period; 210 | int16_t accel; 211 | 212 | // Limit max speed 213 | if (tspeed > MAX_SPEED_Y) 214 | tspeed = MAX_SPEED_Y; 215 | else if (tspeed < -MAX_SPEED_Y) 216 | tspeed = -MAX_SPEED_Y; 217 | //Serial.println(tspeed); 218 | 219 | // We limit acceleration => speed ramp 220 | accel = ((long)acceleration_y*dt)/1000; 221 | if (((long)tspeed-speed_y)>accel) 222 | speed_y += accel; 223 | else if (((long)speed_y-tspeed)>accel) 224 | speed_y -= accel; 225 | else 226 | speed_y = tspeed; 227 | 228 | // Check if we need to change the direction pins 229 | if ((speed_y==0)&&(dir_y!=0)) 230 | dir_y = 0; 231 | else if ((speed_y>0)&&(dir_y!=1)) 232 | { 233 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 234 | CLR(PORTF,7); 235 | #else 236 | SET(PORTF,7); 237 | #endif 238 | 239 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 240 | CLR(PORTL,1); 241 | #else 242 | SET(PORTL,1); 243 | #endif 244 | 245 | dir_y = 1; 246 | } 247 | else if ((speed_y<0)&&(dir_y!=-1)) 248 | { 249 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 250 | SET(PORTF,7); 251 | #else 252 | CLR(PORTF,7); 253 | #endif 254 | 255 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 256 | SET(PORTL,1); 257 | #else 258 | CLR(PORTL,1); 259 | #endif 260 | 261 | dir_y = -1; 262 | } 263 | 264 | if (speed_y==0) 265 | timer_period = ZERO_SPEED; 266 | else if (speed_y>0) 267 | timer_period = 2000000/speed_y; // 2Mhz timer 268 | else 269 | timer_period = 2000000/-speed_y; 270 | 271 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 272 | timer_period = ZERO_SPEED; 273 | 274 | OCR3A = timer_period; 275 | // Check if we need to reset the timer... 276 | if (TCNT3 > OCR3A) 277 | TCNT3 = 0; 278 | } 279 | 280 | // Set speed on each axis in steps/sec 281 | void setSpeedS(int target_sx, int target_sy) 282 | { 283 | target_sx = constrain(target_sx,0,MAX_SPEED_X); 284 | target_sy = constrain(target_sy,0,MAX_SPEED_Y); 285 | target_speed_x = target_sx; 286 | target_speed_y = target_sy; 287 | } 288 | 289 | // set Robot position in mm. 290 | // This function check for valid robot positions values 291 | // Convert from mm units to steps 292 | void setPosition(int target_x_mm_new, int target_y_mm_new) 293 | { 294 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 295 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 296 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 297 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 298 | } 299 | 300 | // set Robot position in mm. 301 | // This function check for valid robot positions values 302 | // Convert from mm units to steps 303 | void setPosition_straight(int target_x_mm_new, int target_y_mm_new) 304 | { 305 | int old_target_position_x; 306 | int old_target_position_y; 307 | int diff_x; 308 | int diff_y; 309 | 310 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 311 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 312 | old_target_position_x = target_position_x; 313 | old_target_position_y = target_position_y; 314 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 315 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 316 | // Speed adjust to draw straight lines 317 | diff_x = myAbs(target_position_x - old_target_position_x); 318 | diff_y = myAbs(target_position_y - old_target_position_y); 319 | if (diff_x > diff_y) // Wich axis should be slower? 320 | { 321 | com_speed_x = target_speed_x; 322 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 323 | setSpeedS(com_speed_x,com_speed_y); 324 | } 325 | else 326 | { 327 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 328 | com_speed_y = target_speed_y; 329 | setSpeedS(com_speed_x,com_speed_y); 330 | } 331 | } 332 | 333 | // set Robot position in 1/10 mm. 334 | // This function check for valid robot positions values 335 | // Convert from 1/10 mm units to steps 336 | // This function moves the robot in a straight line 337 | void setPosition_mm10_straight(int target_x_mm_new, int target_y_mm_new) 338 | { 339 | int old_target_position_x; 340 | int old_target_position_y; 341 | int diff_x; 342 | int diff_y; 343 | 344 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X*10,ROBOT_MAX_X*10); 345 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y*10,ROBOT_MAX_Y*10); 346 | old_target_position_x = target_position_x; 347 | old_target_position_y = target_position_y; 348 | target_position_x = (float)target_x_mm*X_AXIS_STEPS_PER_UNIT/10.0; 349 | target_position_y = (float)target_y_mm*Y_AXIS_STEPS_PER_UNIT/10.0; 350 | // Speed adjust to draw straight lines 351 | diff_x = myAbs(target_position_x - old_target_position_x); 352 | diff_y = myAbs(target_position_y - old_target_position_y); 353 | if (diff_x > diff_y) // Wich axis should be slower? 354 | { 355 | com_speed_x = target_speed_x; 356 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 357 | setSpeedS(com_speed_x,com_speed_y); 358 | } 359 | else 360 | { 361 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 362 | com_speed_y = target_speed_y; 363 | setSpeedS(com_speed_x,com_speed_y); 364 | } 365 | } 366 | 367 | 368 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_X/AHR_Motor_Test_X.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // MOTOR TEST X AXIS! 4 | // Start centered on X and the lowest position of Y 5 | // If you change some parameter on Configuration.h 6 | // remember to copy it to the final project AHRobot. 7 | 8 | // Author: Jose Julio (@jjdrones) 9 | // Hardware: Arduino MEGA + Ramps 1.4 10 | // Date: 21/01/2014 11 | // Updated: 24/03/2014 12 | // Version: 1.01 13 | // Project page : Spanish: English: 14 | // GIT repository: 15 | // License: Open Software GPL License 16 | 17 | // ROBOT and USER configuration parameters 18 | #include "Configuration.h" 19 | #include "Definitions.h" // Variable definitions 20 | 21 | void setup() 22 | { 23 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 24 | // X MOTOR 25 | // X-STEP: A0 (PF0) 26 | // X-DIR: A1 (PF1) 27 | // X-ENABLE: D38 (PD7) 28 | // Y MOTOR (Y-LEFT) 29 | // Y-STEP: A6 (PF6) 30 | // Y-DIR: A7 (PF7) 31 | // Y-ENABLE: A2 (PF2) 32 | // Z MOTOR (Y-RIGHT) 33 | // Z-STEP: D46 (PL3) 34 | // Z-DIR: D48 (PL1) 35 | // Z-ENABLE: A8 (PK0) 36 | 37 | // STEPPER PINS 38 | // X_AXIS 39 | pinMode(38,OUTPUT); // ENABLE MOTOR 40 | pinMode(A0,OUTPUT); // STEP MOTOR 41 | pinMode(A1,OUTPUT); // DIR MOTOR 42 | // Y_AXIS (Y-LEFT) 43 | pinMode(A2,OUTPUT); // ENABLE MOTOR 44 | pinMode(A6,OUTPUT); // STEP MOTOR 45 | pinMode(A7,OUTPUT); // DIR MOTOR 46 | // Z_AXIS (Y-RIGHT) 47 | pinMode(A8,OUTPUT); // ENABLE MOTOR 48 | pinMode(46,OUTPUT); // STEP MOTOR 49 | pinMode(48,OUTPUT); // DIR MOTOR 50 | 51 | pinMode(A3,OUTPUT); // DEBUG PIN FOR OSCILLOSCOPE TIME MEASURES 52 | 53 | pinMode(19,INPUT); // RX1 Serial Port 1 54 | pinMode(18,OUTPUT); // TX1 55 | 56 | //FANS and LEDS 57 | pinMode(8,OUTPUT); 58 | pinMode(9,OUTPUT); 59 | pinMode(10,OUTPUT); 60 | pinMode(13,OUTPUT); 61 | 62 | // Disable Motors 63 | digitalWrite(38,HIGH); 64 | digitalWrite(A2,HIGH); 65 | digitalWrite(A8,HIGH); 66 | 67 | Serial.begin(115200); 68 | Serial.println("AHR Robot X MOTOR TEST v1.01"); 69 | Serial.println("Initializing robot..."); 70 | Serial.print("Free Memory: "); 71 | Serial.print(freeRam()); 72 | Serial.println(); 73 | delay(2500); 74 | 75 | //LED blink 76 | for (uint8_t k=0;k<4;k++) 77 | { 78 | digitalWrite(13,HIGH); 79 | delay(300); 80 | digitalWrite(13,LOW); 81 | delay(300); 82 | } 83 | 84 | // We use TIMER 1 for stepper motor X AXIS and Timer 3 for Y AXIS 85 | // STEPPER MOTORS INITIALIZATION 86 | // TIMER1 CTC MODE 87 | TCCR1B &= ~(1<=1000) // 1Khz loop 179 | { 180 | //dt = (timer_value-timer_old)/1000; 181 | timer_old = timer_value; 182 | loop_counter++; 183 | 184 | if (loop_counter == 2000) 185 | { 186 | Serial.println("Moving the robot to +15cm in X"); 187 | com_pos_x = ROBOT_CENTER_X + 150; 188 | setSpeedS(MAX_SPEED_X/8,MAX_SPEED_Y/8); 189 | setPosition(com_pos_x,com_pos_y); 190 | } 191 | if (loop_counter == 4000) 192 | { 193 | Serial.println("Moving the robot to -15cm in X"); 194 | com_pos_x = ROBOT_CENTER_X - 150; 195 | setSpeedS(MAX_SPEED_X/4,MAX_SPEED_Y/4); 196 | setPosition(com_pos_x,com_pos_y); 197 | } 198 | if (loop_counter == 6000) 199 | { 200 | Serial.println("Moving the robot to +15cm in X"); 201 | com_pos_x = ROBOT_CENTER_X + 150; 202 | setSpeedS(MAX_SPEED_X/2,MAX_SPEED_Y/2); 203 | setPosition(com_pos_x,com_pos_y); 204 | } 205 | if (loop_counter == 8000) 206 | { 207 | Serial.println("Moving the robot to -15cm in X"); 208 | Serial.println("Max speed"); 209 | com_pos_x = ROBOT_CENTER_X - 150; 210 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 211 | setPosition(com_pos_x,com_pos_y); 212 | } 213 | if (loop_counter == 10000) 214 | { 215 | Serial.println("Returning to center"); 216 | com_pos_x = ROBOT_CENTER_X; 217 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 218 | setPosition(com_pos_x,com_pos_y); 219 | } 220 | if (loop_counter == 12000) 221 | { 222 | Serial.println("Quick movements"); 223 | com_pos_x = ROBOT_CENTER_X+150; 224 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 225 | setPosition(com_pos_x,com_pos_y); 226 | } 227 | if (loop_counter == 12100) 228 | { 229 | Serial.println("Quick movements"); 230 | com_pos_x = ROBOT_CENTER_X-150; 231 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 232 | setPosition(com_pos_x,com_pos_y); 233 | } 234 | if (loop_counter == 12200) 235 | { 236 | Serial.println("Quick movements"); 237 | com_pos_x = ROBOT_CENTER_X+150; 238 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 239 | setPosition(com_pos_x,com_pos_y); 240 | } 241 | if (loop_counter == 12300) 242 | { 243 | Serial.println("Quick movements"); 244 | com_pos_x = ROBOT_CENTER_X-150; 245 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 246 | setPosition(com_pos_x,com_pos_y); 247 | } 248 | if (loop_counter == 12500) 249 | { 250 | Serial.println("Returning to center"); 251 | com_pos_x = ROBOT_CENTER_X; 252 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 253 | setPosition(com_pos_x,com_pos_y); 254 | } 255 | 256 | positionControl(); 257 | } // 1Khz loop 258 | } 259 | 260 | 261 | 262 | 263 | 264 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_X/Configuration.h: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // USER CONFIGURATIONS HERE 4 | // ROBOT DIMENSIONS, MAX SPEED, MAX ACCELERATION, CALIBRATION 5 | 6 | // THIS VALUES DEPENDS ON THE VOLTAGE, MOTORS, PULLEYS AND ROBOT CONSTRUCTION 7 | // RECOMMENDED VALUES FOR 12V POWER SUPPLY 8 | #define MIN_ACCEL_X 60 9 | #define MAX_ACCEL_X 300 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 10 | #define MIN_ACCEL_Y 60 11 | #define MAX_ACCEL_Y 145 //140//220 12 | #define MAX_SPEED_X 25000 //max 25000 for 12V // Maximun speed in steps/seg 13 | #define MAX_SPEED_Y 25000 14 | 15 | // RECOMMENDED VALUES FOR 15V POWER SUPPLY 16 | //#define MIN_ACCEL_X 40 17 | //#define MAX_ACCEL_X 380 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 18 | //#define MIN_ACCEL_Y 70 19 | //#define MAX_ACCEL_Y 200 //140//220 20 | //#define MAX_SPEED_X 28000 //max 25000 for 12V // Maximun speed in steps/seg 21 | //#define MAX_SPEED_Y 28000 22 | 23 | 24 | // This is for the Accel ramp implementation (to smooth the intial acceleration), simplified S-profile 25 | #define ACCEL_RAMP_MIN 2500 // The S profile is generated up to this speed 26 | #define ACCEL_RAMP_MAX 10000 27 | 28 | // UNCOMMENT THIS LINES TO INVERT MOTORS 29 | #define INVERT_X_AXIS 1 30 | //#define INVERT_Y_AXIS 1 //Y-LEFT 31 | //#define INVERT_Z_AXIS 1 //Y_RIGHT 32 | 33 | // Geometric calibration. 34 | // This depends on the pulley teeth. For 42 teeth GT2 => 19, for 40 teeth GT2 => 20, for 16 teeth T5 => 20 35 | #define X_AXIS_STEPS_PER_UNIT 19 // With 42 teeth GT2 pulley and 1/8 microstepping on drivers 36 | #define Y_AXIS_STEPS_PER_UNIT 19 // 200*8 = 1600 steps/rev = 1600/42teeth*2mm = 19.047, using 19 is an error of 1mm every 40cm not too much! and we use int operations... 37 | 38 | // Absolute Min and Max robot positions in mm (measured from center of robot pusher) 39 | #define ROBOT_MIN_X 100 40 | #define ROBOT_MIN_Y 80 41 | #define ROBOT_MAX_X 500 42 | #define ROBOT_MAX_Y 400 43 | 44 | // This is the center of the table. All units in milimeters 45 | #define ROBOT_CENTER_X 300 // Center of robot. The table is 600x1000mm, so center is 300,500 46 | #define ROBOT_CENTER_Y 500 47 | 48 | // Initial robot position in mm 49 | // The robot must be at this position at start time 50 | // Default: Centered in X and minimun position in Y 51 | #define ROBOT_INITIAL_POSITION_X 300 52 | #define ROBOT_INITIAL_POSITION_Y 45 // Measured from center of the robot pusher to the table border 53 | 54 | // Robot defense and attack lines 55 | #define ROBOT_DEFENSE_POSITION 95 56 | #define ROBOT_DEFENSE_ATTACK_POSITION 220 57 | 58 | #define POSITION_TOLERANCE 5 // 5 steps 59 | 60 | // PS3 Camera pixels 61 | // NOTE: We are using the camera at 320x240 but we are outputing a 640x480 pixel equivalent position 62 | #define CAM_PIX_WIDTH 640 63 | #define CAM_PIX_HEIGHT 480 64 | #define CAM_PIX_CENTER_X 320 65 | #define CAM_PIX_CENTER_Y 240 66 | 67 | // IMPORTANT: THIS VALUE IS FOR A 87.5 cm CARBON TUBE!! 68 | // Camera geometric calibration 69 | // Measure two known position (in mm) calculare the pixels on camera and generate divide: mm/pix 70 | #define CAM_PIX_TO_MM 1.48 // Camera scale To calculate, you need to measure two points with known coordinates 71 | 72 | // CORRECTION OF MISSING STEPS ON MOTORS 73 | // Coment this lines if you don´t want to make the corrections 74 | #define CORRECT_MISSING_STEPS_X 1 75 | #define CORRECT_MISSING_STEPS_Y 1 76 | 77 | #define MISSING_STEPS_MAX_ERROR_X 8 78 | #define MISSING_STEPS_MAX_ERROR_Y 10 79 | #define ROBOT_POSITION_CAMERA_CORRECTION_Y -20 // Correction of the position of the camera because the camera point of view and mark position 80 | 81 | // AIR HOCKEY TABLE FANS SPEED 82 | // USE 255 for FULL SPEED (if you use 15V power supply 180 is ok) 83 | #define FAN1_SPEED 255 //180 84 | #define FAN2_SPEED 255 //180 85 | 86 | // Utils (don´t modify) 87 | #define CLR(x,y) (x&=(~(1< 4 | 5 | // Variable definitions 6 | 7 | // Log and Timer variables 8 | long loop_counter; 9 | long timer_old; 10 | long timer_packet_old; 11 | long timer_value; 12 | int debug_counter; 13 | 14 | uint32_t micros_old; 15 | 16 | // We have 2 axis => 2 motor controls 0=X axis 1=Y axis (Y AXIS HAS 2 MOTORS Left and Right) 17 | int16_t speed_m[2]; // Actual speed of motors 18 | uint8_t dir_m[2]; // Actual direction of steppers motors 19 | 20 | uint16_t counter_m[2]; // counters for periods 21 | uint16_t period_m[2][8]; // Eight subperiods 22 | uint8_t period_m_index[2]; // index for subperiods 23 | 24 | // kinematic variables 25 | // position, speed and acceleration are in step units 26 | volatile int16_t position_x; // This variables are modified inside the Timer interrupts 27 | volatile int16_t position_y; 28 | 29 | int16_t speed_x; 30 | int16_t speed_y; 31 | int16_t max_speed_x; 32 | int16_t max_speed_y; 33 | 34 | int8_t dir_x; //(dir=1 positive, dir=-1 negative) 35 | int8_t dir_y; 36 | int16_t target_position_x; 37 | int16_t target_position_y; 38 | int16_t target_speed_x; 39 | int16_t target_speed_y; 40 | int16_t max_acceleration_x = MAX_ACCEL_X; // default maximun acceleration 41 | int16_t max_acceleration_y = MAX_ACCEL_Y; 42 | int16_t acceleration_x = MAX_ACCEL_X; 43 | int16_t acceleration_y = MAX_ACCEL_Y; 44 | int16_t accel_ramp = ACCEL_RAMP_MIN; 45 | 46 | int16_t pos_stop_x; 47 | int16_t pos_stop_y; 48 | 49 | uint16_t com_pos_x; 50 | uint16_t com_pos_y; 51 | uint16_t com_speed_x; 52 | uint16_t com_speed_y; 53 | uint16_t target_x_mm; 54 | uint16_t target_y_mm; 55 | int16_t user_speed_x; 56 | int16_t user_speed_y; 57 | int16_t filt_user_speed_x; 58 | int16_t filt_user_speed_y; 59 | 60 | 61 | // Some util functions... 62 | int freeRam () { 63 | extern int __heap_start, *__brkval; 64 | int v; 65 | return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 66 | } 67 | 68 | int16_t myAbs(int16_t param) 69 | { 70 | if (param<0) 71 | return -param; 72 | else 73 | return param; 74 | } 75 | 76 | int sign(int val) 77 | { 78 | if (val<0) 79 | return(-1); 80 | else 81 | return(1); 82 | } 83 | 84 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_X/Steppers.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // STEPPERS MOTOR CONTROL 4 | // Updated: Now it supports DRV8825 drivers 5 | // SPEED, ACCELERATION AND POSITION CONTROL using Arduino 16 bit Timer interrupts 6 | 7 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 8 | // X MOTOR 9 | // X-STEP: A0 (PF0) 10 | // X-DIR: A1 (PF1) 11 | // X-ENABLE: D38 (PD7) 12 | // Y MOTOR (Y-LEFT) 13 | // Y-STEP: A6 (PF6) 14 | // Y-DIR: A7 (PF7) 15 | // Y-ENABLE: A2 (PF2) 16 | // Z MOTOR (Y-RIGHT) 17 | // Z-STEP: D46 (PL3) 18 | // Z-DIR: D48 (PL1) 19 | // Z-ENABLE: A8 (PK0) 20 | // Y and Z motors controls the Y axis of the robot (same output) 21 | 22 | // We control the speed of the motors with interrupts (Timer1 and Timer3) tested up to 25Khz. 23 | // The position of the motor is controlled at 1Khz (in the main loop) 24 | 25 | 26 | // TIMER 1 : STEPPER MOTOR SPEED CONTROL X-AXIS 27 | ISR(TIMER1_COMPA_vect) 28 | { 29 | if (dir_x==0) 30 | return; 31 | 32 | SET(PORTF,0); // STEP X-AXIS 33 | position_x += dir_x; 34 | __asm__ __volatile__ ( 35 | "nop" "\n\t" 36 | "nop" "\n\t" 37 | "nop" "\n\t" 38 | "nop" "\n\t" 39 | "nop" "\n\t" 40 | "nop" "\n\t" 41 | "nop" "\n\t" 42 | "nop" "\n\t" 43 | "nop" "\n\t" 44 | "nop" "\n\t" 45 | "nop" "\n\t" 46 | "nop" "\n\t" 47 | "nop" "\n\t" 48 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 49 | CLR(PORTF,0); 50 | } 51 | 52 | // TIMER 3 : STEPPER MOTOR SPEED CONTROL Y-AXIS (2 Motors, left and right) 53 | ISR(TIMER3_COMPA_vect) 54 | { 55 | if (dir_y==0) 56 | return; 57 | 58 | SET(PORTF,6); // STEP Y-AXIS (Y-left) 59 | SET(PORTL,3); // STEP Z-AXIS (Y-right) 60 | position_y += dir_y; 61 | __asm__ __volatile__ ( 62 | "nop" "\n\t" 63 | "nop" "\n\t" 64 | "nop" "\n\t" 65 | "nop" "\n\t" 66 | "nop" "\n\t" 67 | "nop" "\n\t" 68 | "nop" "\n\t" 69 | "nop" "\n\t" 70 | "nop" "\n\t" 71 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 72 | CLR(PORTF,6); 73 | CLR(PORTL,3); 74 | } 75 | 76 | // We use a ramp for acceleration and deceleration 77 | // To calculate the point we should start to decelerate we use this formula: 78 | // stop_position = actual_posicion + (actual_speed*actual_speed)/(2*max_deceleration) 79 | // Beacuase we are using a S-profile we apply a correction factor to the above formula (0.85) 80 | // Input parameters: 81 | // target_position_x 82 | // target_speed_x 83 | // max_acceleration_x 84 | 85 | void positionControl() 86 | { 87 | //int16_t pos_stop; 88 | int32_t temp; 89 | uint32_t timer; 90 | int16_t dt; 91 | 92 | SET(PORTF,3); // for external timing debug 93 | timer = micros(); 94 | // dt = delta time in microseconds... 95 | dt = constrain(timer-micros_old,0,2000); // Limit dt (it should be around 1000 most times) 96 | //Serial.println(dt); 97 | micros_old = timer; 98 | 99 | // We use an acceleration ramp to imitate an S-curve profile at the begining and end (depend on speed) 100 | acceleration_x = map(abs(speed_x),0,accel_ramp,MIN_ACCEL_X,max_acceleration_x); 101 | acceleration_x = constrain(acceleration_x,MIN_ACCEL_X,max_acceleration_x); 102 | 103 | acceleration_y = map(abs(speed_y),0,accel_ramp,MIN_ACCEL_Y,max_acceleration_y); 104 | acceleration_y = constrain(acceleration_y,MIN_ACCEL_Y,max_acceleration_y); 105 | 106 | // X AXIS 107 | temp = (long)speed_x*speed_x; 108 | temp = temp/(1800*(long)acceleration_x); // 2000*0.85 = 1700 0.85 is a compensation for deceleration ramp (S-curve) 109 | pos_stop_x = position_x + sign(speed_x)*temp; 110 | if (target_position_x>position_x) // Positive move 111 | { 112 | if (pos_stop_x >= target_position_x) // Start decelerating? 113 | setMotorXSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 114 | else 115 | setMotorXSpeed(target_speed_x,dt); // The aceleration ramp is done inside the setSpeed function 116 | } 117 | else // Negative move 118 | { 119 | if (pos_stop_x <= target_position_x) // Start decelerating? 120 | setMotorXSpeed(0,dt); 121 | else 122 | setMotorXSpeed(-target_speed_x,dt); 123 | } 124 | 125 | // Y AXIS 126 | temp = (long)speed_y*speed_y; 127 | temp = temp/(1800*(long)acceleration_y); 128 | pos_stop_y = position_y + sign(speed_y)*temp; 129 | if (target_position_y>position_y) // Positive move 130 | { 131 | if (pos_stop_y >= target_position_y) // Start decelerating? 132 | setMotorYSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 133 | else 134 | setMotorYSpeed(target_speed_y,dt); // The aceleration ramp is done inside the setSpeed function 135 | } 136 | else // Negative move 137 | { 138 | if (pos_stop_y <= target_position_y) // Start decelerating? 139 | setMotorYSpeed(0,dt); 140 | else 141 | setMotorYSpeed(-target_speed_y,dt); 142 | } 143 | CLR(PORTF,3); // for external timing debug 144 | } 145 | 146 | // Speed could be positive or negative 147 | void setMotorXSpeed(int16_t tspeed, int16_t dt) 148 | { 149 | long timer_period; 150 | int16_t accel; 151 | 152 | // Limit max speed 153 | if (tspeed > MAX_SPEED_X) 154 | tspeed = MAX_SPEED_X; 155 | else if (tspeed < -MAX_SPEED_X) 156 | tspeed = -MAX_SPEED_X; 157 | //Serial.println(tspeed); 158 | 159 | // We limit acceleration => speed ramp 160 | accel = ((long)acceleration_x*dt)/1000; // We divide by 1000 because dt are in microseconds 161 | if (((long)tspeed-speed_x)>accel) // We use long here to avoid overflow on the operation 162 | speed_x += accel; 163 | else if (((long)speed_x-tspeed)>accel) 164 | speed_x -= accel; 165 | else 166 | speed_x = tspeed; 167 | 168 | // Check if we need to change the direction pins 169 | if ((speed_x==0)&&(dir_x!=0)) 170 | dir_x = 0; 171 | else if ((speed_x>0)&&(dir_x!=1)) 172 | { 173 | #ifdef INVERT_X_AXIS 174 | CLR(PORTF,1); // X-DIR 175 | #else 176 | SET(PORTF,1); 177 | #endif 178 | dir_x = 1; 179 | } 180 | else if ((speed_x<0)&&(dir_x!=-1)) 181 | { 182 | #ifdef INVERT_X_AXIS 183 | SET(PORTF,1); 184 | #else 185 | CLR(PORTF,1); 186 | #endif 187 | dir_x = -1; 188 | } 189 | 190 | if (speed_x==0) 191 | timer_period = ZERO_SPEED; 192 | else if (speed_x>0) 193 | timer_period = 2000000/speed_x; // 2Mhz timer 194 | else 195 | timer_period = 2000000/-speed_x; 196 | 197 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 198 | timer_period = ZERO_SPEED; 199 | 200 | OCR1A = timer_period; 201 | // Check if we need to reset the timer... 202 | if (TCNT1 > OCR1A) 203 | TCNT1 = 0; 204 | } 205 | 206 | // Speed could be positive or negative 207 | void setMotorYSpeed(int16_t tspeed,int16_t dt) 208 | { 209 | long timer_period; 210 | int16_t accel; 211 | 212 | // Limit max speed 213 | if (tspeed > MAX_SPEED_Y) 214 | tspeed = MAX_SPEED_Y; 215 | else if (tspeed < -MAX_SPEED_Y) 216 | tspeed = -MAX_SPEED_Y; 217 | //Serial.println(tspeed); 218 | 219 | // We limit acceleration => speed ramp 220 | accel = ((long)acceleration_y*dt)/1000; 221 | if (((long)tspeed-speed_y)>accel) 222 | speed_y += accel; 223 | else if (((long)speed_y-tspeed)>accel) 224 | speed_y -= accel; 225 | else 226 | speed_y = tspeed; 227 | 228 | // Check if we need to change the direction pins 229 | if ((speed_y==0)&&(dir_y!=0)) 230 | dir_y = 0; 231 | else if ((speed_y>0)&&(dir_y!=1)) 232 | { 233 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 234 | CLR(PORTF,7); 235 | #else 236 | SET(PORTF,7); 237 | #endif 238 | 239 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 240 | CLR(PORTL,1); 241 | #else 242 | SET(PORTL,1); 243 | #endif 244 | 245 | dir_y = 1; 246 | } 247 | else if ((speed_y<0)&&(dir_y!=-1)) 248 | { 249 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 250 | SET(PORTF,7); 251 | #else 252 | CLR(PORTF,7); 253 | #endif 254 | 255 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 256 | SET(PORTL,1); 257 | #else 258 | CLR(PORTL,1); 259 | #endif 260 | 261 | dir_y = -1; 262 | } 263 | 264 | if (speed_y==0) 265 | timer_period = ZERO_SPEED; 266 | else if (speed_y>0) 267 | timer_period = 2000000/speed_y; // 2Mhz timer 268 | else 269 | timer_period = 2000000/-speed_y; 270 | 271 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 272 | timer_period = ZERO_SPEED; 273 | 274 | OCR3A = timer_period; 275 | // Check if we need to reset the timer... 276 | if (TCNT3 > OCR3A) 277 | TCNT3 = 0; 278 | } 279 | 280 | // Set speed on each axis in steps/sec 281 | void setSpeedS(int target_sx, int target_sy) 282 | { 283 | target_sx = constrain(target_sx,0,MAX_SPEED_X); 284 | target_sy = constrain(target_sy,0,MAX_SPEED_Y); 285 | target_speed_x = target_sx; 286 | target_speed_y = target_sy; 287 | } 288 | 289 | // set Robot position in mm. 290 | // This function check for valid robot positions values 291 | // Convert from mm units to steps 292 | void setPosition(int target_x_mm_new, int target_y_mm_new) 293 | { 294 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 295 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 296 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 297 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 298 | } 299 | 300 | // set Robot position in mm. 301 | // This function check for valid robot positions values 302 | // Convert from mm units to steps 303 | void setPosition_straight(int target_x_mm_new, int target_y_mm_new) 304 | { 305 | int old_target_position_x; 306 | int old_target_position_y; 307 | int diff_x; 308 | int diff_y; 309 | 310 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 311 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 312 | old_target_position_x = target_position_x; 313 | old_target_position_y = target_position_y; 314 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 315 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 316 | // Speed adjust to draw straight lines 317 | diff_x = myAbs(target_position_x - old_target_position_x); 318 | diff_y = myAbs(target_position_y - old_target_position_y); 319 | if (diff_x > diff_y) // Wich axis should be slower? 320 | { 321 | com_speed_x = target_speed_x; 322 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 323 | setSpeedS(com_speed_x,com_speed_y); 324 | } 325 | else 326 | { 327 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 328 | com_speed_y = target_speed_y; 329 | setSpeedS(com_speed_x,com_speed_y); 330 | } 331 | } 332 | 333 | // set Robot position in 1/10 mm. 334 | // This function check for valid robot positions values 335 | // Convert from 1/10 mm units to steps 336 | // This function moves the robot in a straight line 337 | void setPosition_mm10_straight(int target_x_mm_new, int target_y_mm_new) 338 | { 339 | int old_target_position_x; 340 | int old_target_position_y; 341 | int diff_x; 342 | int diff_y; 343 | 344 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X*10,ROBOT_MAX_X*10); 345 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y*10,ROBOT_MAX_Y*10); 346 | old_target_position_x = target_position_x; 347 | old_target_position_y = target_position_y; 348 | target_position_x = (float)target_x_mm*X_AXIS_STEPS_PER_UNIT/10.0; 349 | target_position_y = (float)target_y_mm*Y_AXIS_STEPS_PER_UNIT/10.0; 350 | // Speed adjust to draw straight lines 351 | diff_x = myAbs(target_position_x - old_target_position_x); 352 | diff_y = myAbs(target_position_y - old_target_position_y); 353 | if (diff_x > diff_y) // Wich axis should be slower? 354 | { 355 | com_speed_x = target_speed_x; 356 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 357 | setSpeedS(com_speed_x,com_speed_y); 358 | } 359 | else 360 | { 361 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 362 | com_speed_y = target_speed_y; 363 | setSpeedS(com_speed_x,com_speed_y); 364 | } 365 | } 366 | 367 | 368 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_Y/AHR_Motor_Test_Y.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // MOTOR TEST Y AXIS! 4 | // Start centered on X and the lowest position of Y 5 | // If you change some parameter on Configuration.h 6 | // remember to copy it to the final project AHRobot. 7 | 8 | // Author: Jose Julio (@jjdrones) 9 | // Hardware: Arduino MEGA + Ramps 1.4 10 | // Date: 21/01/2014 11 | // Updated: 24/03/2014 12 | // Version: 1.01 13 | // Project page : Spanish: English: 14 | // GIT repository: 15 | // License: Open Software GPL License 16 | 17 | // ROBOT and USER configuration parameters 18 | #include "Configuration.h" 19 | #include "Definitions.h" // Variable definitions 20 | 21 | void setup() 22 | { 23 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 24 | // X MOTOR 25 | // X-STEP: A0 (PF0) 26 | // X-DIR: A1 (PF1) 27 | // X-ENABLE: D38 (PD7) 28 | // Y MOTOR (Y-LEFT) 29 | // Y-STEP: A6 (PF6) 30 | // Y-DIR: A7 (PF7) 31 | // Y-ENABLE: A2 (PF2) 32 | // Z MOTOR (Y-RIGHT) 33 | // Z-STEP: D46 (PL3) 34 | // Z-DIR: D48 (PL1) 35 | // Z-ENABLE: A8 (PK0) 36 | 37 | // STEPPER PINS 38 | // X_AXIS 39 | pinMode(38,OUTPUT); // ENABLE MOTOR 40 | pinMode(A0,OUTPUT); // STEP MOTOR 41 | pinMode(A1,OUTPUT); // DIR MOTOR 42 | // Y_AXIS (Y-LEFT) 43 | pinMode(A2,OUTPUT); // ENABLE MOTOR 44 | pinMode(A6,OUTPUT); // STEP MOTOR 45 | pinMode(A7,OUTPUT); // DIR MOTOR 46 | // Z_AXIS (Y-RIGHT) 47 | pinMode(A8,OUTPUT); // ENABLE MOTOR 48 | pinMode(46,OUTPUT); // STEP MOTOR 49 | pinMode(48,OUTPUT); // DIR MOTOR 50 | 51 | pinMode(A3,OUTPUT); // DEBUG PIN FOR OSCILLOSCOPE TIME MEASURES 52 | 53 | pinMode(19,INPUT); // RX1 Serial Port 1 54 | pinMode(18,OUTPUT); // TX1 55 | 56 | //FANS and LEDS 57 | pinMode(8,OUTPUT); 58 | pinMode(9,OUTPUT); 59 | pinMode(10,OUTPUT); 60 | pinMode(13,OUTPUT); 61 | 62 | // Disable Motors 63 | digitalWrite(38,HIGH); 64 | digitalWrite(A2,HIGH); 65 | digitalWrite(A8,HIGH); 66 | 67 | Serial.begin(115200); 68 | Serial.println("AHR Robot Y MOTOR TEST v1.01"); 69 | Serial.println("Initializing robot..."); 70 | Serial.print("Free Memory: "); 71 | Serial.print(freeRam()); 72 | Serial.println(); 73 | delay(2500); 74 | 75 | //LED blink 76 | for (uint8_t k=0;k<4;k++) 77 | { 78 | digitalWrite(13,HIGH); 79 | delay(300); 80 | digitalWrite(13,LOW); 81 | delay(300); 82 | } 83 | 84 | // We use TIMER 1 for stepper motor X AXIS and Timer 3 for Y AXIS 85 | // STEPPER MOTORS INITIALIZATION 86 | // TIMER1 CTC MODE 87 | TCCR1B &= ~(1<=1000) // 1Khz loop 179 | { 180 | //dt = (timer_value-timer_old)/1000; 181 | timer_old = timer_value; 182 | loop_counter++; 183 | 184 | if (loop_counter == 2000) 185 | { 186 | Serial.println("Moving the robot to +15cm in Y"); 187 | com_pos_y = ROBOT_MIN_Y + 150; 188 | setSpeedS(MAX_SPEED_X/8,MAX_SPEED_Y/8); 189 | setPosition(com_pos_x,com_pos_y); 190 | } 191 | if (loop_counter == 4000) 192 | { 193 | Serial.println("Moving the robot to back position"); 194 | com_pos_y = ROBOT_MIN_Y; 195 | setSpeedS(MAX_SPEED_X/4,MAX_SPEED_Y/4); 196 | setPosition(com_pos_x,com_pos_y); 197 | } 198 | if (loop_counter == 6000) 199 | { 200 | Serial.println("Moving the robot to +15cm in Y"); 201 | com_pos_y = ROBOT_MIN_Y + 150; 202 | setSpeedS(MAX_SPEED_X/2,MAX_SPEED_Y/2); 203 | setPosition(com_pos_x,com_pos_y); 204 | } 205 | if (loop_counter == 8000) 206 | { 207 | Serial.println("Moving the robot back position"); 208 | Serial.println("Max speed"); 209 | com_pos_y = ROBOT_MIN_Y; 210 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 211 | setPosition(com_pos_x,com_pos_y); 212 | } 213 | if (loop_counter == 10000) 214 | { 215 | /* 216 | Serial.println("Returning to center"); 217 | com_pos_x = ROBOT_CENTER_X; 218 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 219 | setPosition(com_pos_x,com_pos_y); 220 | */ 221 | } 222 | if (loop_counter == 12000) 223 | { 224 | Serial.println("Quick movements"); 225 | com_pos_y = ROBOT_MIN_Y + 150; 226 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 227 | setPosition(com_pos_x,com_pos_y); 228 | } 229 | if (loop_counter == 12100) 230 | { 231 | Serial.println("Quick movements"); 232 | com_pos_y = ROBOT_MIN_Y + 50; 233 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 234 | setPosition(com_pos_x,com_pos_y); 235 | } 236 | if (loop_counter == 12200) 237 | { 238 | Serial.println("Quick movements"); 239 | com_pos_y = ROBOT_MIN_Y + 150; 240 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 241 | setPosition(com_pos_x,com_pos_y); 242 | } 243 | if (loop_counter == 12300) 244 | { 245 | Serial.println("Quick movements"); 246 | com_pos_y = ROBOT_MIN_Y + 50; 247 | setSpeedS(MAX_SPEED_X,MAX_SPEED_Y); 248 | setPosition(com_pos_x,com_pos_y); 249 | } 250 | if (loop_counter == 12500) 251 | { 252 | Serial.println("Returning to initial position"); 253 | com_pos_y = ROBOT_MIN_Y; 254 | setSpeedS(MAX_SPEED_X/2,MAX_SPEED_Y/2); 255 | setPosition(com_pos_x,com_pos_y); 256 | } 257 | 258 | positionControl(); 259 | } // 1Khz loop 260 | } 261 | 262 | 263 | 264 | 265 | 266 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_Y/Configuration.h: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // USER CONFIGURATIONS HERE 4 | // ROBOT DIMENSIONS, MAX SPEED, MAX ACCELERATION, CALIBRATION 5 | 6 | // THIS VALUES DEPENDS ON THE VOLTAGE, MOTORS, PULLEYS AND ROBOT CONSTRUCTION 7 | // RECOMMENDED VALUES FOR 12V POWER SUPPLY 8 | #define MIN_ACCEL_X 60 9 | #define MAX_ACCEL_X 300 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 10 | #define MIN_ACCEL_Y 60 11 | #define MAX_ACCEL_Y 145 //140//220 12 | #define MAX_SPEED_X 25000 //max 25000 for 12V // Maximun speed in steps/seg 13 | #define MAX_SPEED_Y 25000 14 | 15 | // RECOMMENDED VALUES FOR 15V POWER SUPPLY 16 | //#define MIN_ACCEL_X 40 17 | //#define MAX_ACCEL_X 380 //360 //300//320 // Maximun motor acceleration in (steps/seg2)/1000 18 | //#define MIN_ACCEL_Y 70 19 | //#define MAX_ACCEL_Y 200 //140//220 20 | //#define MAX_SPEED_X 28000 //max 25000 for 12V // Maximun speed in steps/seg 21 | //#define MAX_SPEED_Y 28000 22 | 23 | 24 | // This is for the Accel ramp implementation (to smooth the intial acceleration), simplified S-profile 25 | #define ACCEL_RAMP_MIN 2500 // The S profile is generated up to this speed 26 | #define ACCEL_RAMP_MAX 10000 27 | 28 | // UNCOMMENT THIS LINES TO INVERT MOTORS 29 | #define INVERT_X_AXIS 1 30 | //#define INVERT_Y_AXIS 1 //Y-LEFT 31 | //#define INVERT_Z_AXIS 1 //Y_RIGHT 32 | 33 | // Geometric calibration. 34 | // This depends on the pulley teeth. For 42 teeth GT2 => 19, for 40 teeth GT2 => 20, for 16 teeth T5 => 20 35 | #define X_AXIS_STEPS_PER_UNIT 19 // With 42 teeth GT2 pulley and 1/8 microstepping on drivers 36 | #define Y_AXIS_STEPS_PER_UNIT 19 // 200*8 = 1600 steps/rev = 1600/42teeth*2mm = 19.047, using 19 is an error of 1mm every 40cm not too much! and we use int operations... 37 | 38 | // Absolute Min and Max robot positions in mm (measured from center of robot pusher) 39 | #define ROBOT_MIN_X 100 40 | #define ROBOT_MIN_Y 80 41 | #define ROBOT_MAX_X 500 42 | #define ROBOT_MAX_Y 400 43 | 44 | // This is the center of the table. All units in milimeters 45 | #define ROBOT_CENTER_X 300 // Center of robot. The table is 600x1000mm, so center is 300,500 46 | #define ROBOT_CENTER_Y 500 47 | 48 | // Initial robot position in mm 49 | // The robot must be at this position at start time 50 | // Default: Centered in X and minimun position in Y 51 | #define ROBOT_INITIAL_POSITION_X 300 52 | #define ROBOT_INITIAL_POSITION_Y 45 // Measured from center of the robot pusher to the table border 53 | 54 | // Robot defense and attack lines 55 | #define ROBOT_DEFENSE_POSITION 95 56 | #define ROBOT_DEFENSE_ATTACK_POSITION 220 57 | 58 | #define POSITION_TOLERANCE 5 // 5 steps 59 | 60 | // PS3 Camera pixels 61 | // NOTE: We are using the camera at 320x240 but we are outputing a 640x480 pixel equivalent position 62 | #define CAM_PIX_WIDTH 640 63 | #define CAM_PIX_HEIGHT 480 64 | #define CAM_PIX_CENTER_X 320 65 | #define CAM_PIX_CENTER_Y 240 66 | 67 | // IMPORTANT: THIS VALUE IS FOR A 87.5 cm CARBON TUBE!! 68 | // Camera geometric calibration 69 | // Measure two known position (in mm) calculare the pixels on camera and generate divide: mm/pix 70 | #define CAM_PIX_TO_MM 1.48 // Camera scale To calculate, you need to measure two points with known coordinates 71 | 72 | // CORRECTION OF MISSING STEPS ON MOTORS 73 | // Coment this lines if you don´t want to make the corrections 74 | #define CORRECT_MISSING_STEPS_X 1 75 | #define CORRECT_MISSING_STEPS_Y 1 76 | 77 | #define MISSING_STEPS_MAX_ERROR_X 8 78 | #define MISSING_STEPS_MAX_ERROR_Y 10 79 | #define ROBOT_POSITION_CAMERA_CORRECTION_Y -20 // Correction of the position of the camera because the camera point of view and mark position 80 | 81 | // AIR HOCKEY TABLE FANS SPEED 82 | // USE 255 for FULL SPEED (if you use 15V power supply 180 is ok) 83 | #define FAN1_SPEED 255 //180 84 | #define FAN2_SPEED 255 //180 85 | 86 | // Utils (don´t modify) 87 | #define CLR(x,y) (x&=(~(1< 4 | 5 | // Variable definitions 6 | 7 | // Log and Timer variables 8 | long loop_counter; 9 | long timer_old; 10 | long timer_packet_old; 11 | long timer_value; 12 | int debug_counter; 13 | 14 | uint32_t micros_old; 15 | 16 | // We have 2 axis => 2 motor controls 0=X axis 1=Y axis (Y AXIS HAS 2 MOTORS Left and Right) 17 | int16_t speed_m[2]; // Actual speed of motors 18 | uint8_t dir_m[2]; // Actual direction of steppers motors 19 | 20 | uint16_t counter_m[2]; // counters for periods 21 | uint16_t period_m[2][8]; // Eight subperiods 22 | uint8_t period_m_index[2]; // index for subperiods 23 | 24 | // kinematic variables 25 | // position, speed and acceleration are in step units 26 | volatile int16_t position_x; // This variables are modified inside the Timer interrupts 27 | volatile int16_t position_y; 28 | 29 | int16_t speed_x; 30 | int16_t speed_y; 31 | int16_t max_speed_x; 32 | int16_t max_speed_y; 33 | 34 | int8_t dir_x; //(dir=1 positive, dir=-1 negative) 35 | int8_t dir_y; 36 | int16_t target_position_x; 37 | int16_t target_position_y; 38 | int16_t target_speed_x; 39 | int16_t target_speed_y; 40 | int16_t max_acceleration_x = MAX_ACCEL_X; // default maximun acceleration 41 | int16_t max_acceleration_y = MAX_ACCEL_Y; 42 | int16_t acceleration_x = MAX_ACCEL_X; 43 | int16_t acceleration_y = MAX_ACCEL_Y; 44 | int16_t accel_ramp = ACCEL_RAMP_MIN; 45 | 46 | int16_t pos_stop_x; 47 | int16_t pos_stop_y; 48 | 49 | uint16_t com_pos_x; 50 | uint16_t com_pos_y; 51 | uint16_t com_speed_x; 52 | uint16_t com_speed_y; 53 | uint16_t target_x_mm; 54 | uint16_t target_y_mm; 55 | int16_t user_speed_x; 56 | int16_t user_speed_y; 57 | int16_t filt_user_speed_x; 58 | int16_t filt_user_speed_y; 59 | 60 | 61 | // Some util functions... 62 | int freeRam () { 63 | extern int __heap_start, *__brkval; 64 | int v; 65 | return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 66 | } 67 | 68 | int16_t myAbs(int16_t param) 69 | { 70 | if (param<0) 71 | return -param; 72 | else 73 | return param; 74 | } 75 | 76 | int sign(int val) 77 | { 78 | if (val<0) 79 | return(-1); 80 | else 81 | return(1); 82 | } 83 | 84 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_Motor_Test_Y/Steppers.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // STEPPERS MOTOR CONTROL 4 | // Updated: Now it supports DRV8825 drivers 5 | // SPEED, ACCELERATION AND POSITION CONTROL using Arduino 16 bit Timer interrupts 6 | 7 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 8 | // X MOTOR 9 | // X-STEP: A0 (PF0) 10 | // X-DIR: A1 (PF1) 11 | // X-ENABLE: D38 (PD7) 12 | // Y MOTOR (Y-LEFT) 13 | // Y-STEP: A6 (PF6) 14 | // Y-DIR: A7 (PF7) 15 | // Y-ENABLE: A2 (PF2) 16 | // Z MOTOR (Y-RIGHT) 17 | // Z-STEP: D46 (PL3) 18 | // Z-DIR: D48 (PL1) 19 | // Z-ENABLE: A8 (PK0) 20 | // Y and Z motors controls the Y axis of the robot (same output) 21 | 22 | // We control the speed of the motors with interrupts (Timer1 and Timer3) tested up to 25Khz. 23 | // The position of the motor is controlled at 1Khz (in the main loop) 24 | 25 | 26 | // TIMER 1 : STEPPER MOTOR SPEED CONTROL X-AXIS 27 | ISR(TIMER1_COMPA_vect) 28 | { 29 | if (dir_x==0) 30 | return; 31 | 32 | SET(PORTF,0); // STEP X-AXIS 33 | position_x += dir_x; 34 | __asm__ __volatile__ ( 35 | "nop" "\n\t" 36 | "nop" "\n\t" 37 | "nop" "\n\t" 38 | "nop" "\n\t" 39 | "nop" "\n\t" 40 | "nop" "\n\t" 41 | "nop" "\n\t" 42 | "nop" "\n\t" 43 | "nop" "\n\t" 44 | "nop" "\n\t" 45 | "nop" "\n\t" 46 | "nop" "\n\t" 47 | "nop" "\n\t" 48 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 49 | CLR(PORTF,0); 50 | } 51 | 52 | // TIMER 3 : STEPPER MOTOR SPEED CONTROL Y-AXIS (2 Motors, left and right) 53 | ISR(TIMER3_COMPA_vect) 54 | { 55 | if (dir_y==0) 56 | return; 57 | 58 | SET(PORTF,6); // STEP Y-AXIS (Y-left) 59 | SET(PORTL,3); // STEP Z-AXIS (Y-right) 60 | position_y += dir_y; 61 | __asm__ __volatile__ ( 62 | "nop" "\n\t" 63 | "nop" "\n\t" 64 | "nop" "\n\t" 65 | "nop" "\n\t" 66 | "nop" "\n\t" 67 | "nop" "\n\t" 68 | "nop" "\n\t" 69 | "nop" "\n\t" 70 | "nop" "\n\t" 71 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 72 | CLR(PORTF,6); 73 | CLR(PORTL,3); 74 | } 75 | 76 | // We use a ramp for acceleration and deceleration 77 | // To calculate the point we should start to decelerate we use this formula: 78 | // stop_position = actual_posicion + (actual_speed*actual_speed)/(2*max_deceleration) 79 | // Beacuase we are using a S-profile we apply a correction factor to the above formula (0.85) 80 | // Input parameters: 81 | // target_position_x 82 | // target_speed_x 83 | // max_acceleration_x 84 | 85 | void positionControl() 86 | { 87 | //int16_t pos_stop; 88 | int32_t temp; 89 | uint32_t timer; 90 | int16_t dt; 91 | 92 | SET(PORTF,3); // for external timing debug 93 | timer = micros(); 94 | // dt = delta time in microseconds... 95 | dt = constrain(timer-micros_old,0,2000); // Limit dt (it should be around 1000 most times) 96 | //Serial.println(dt); 97 | micros_old = timer; 98 | 99 | // We use an acceleration ramp to imitate an S-curve profile at the begining and end (depend on speed) 100 | acceleration_x = map(abs(speed_x),0,accel_ramp,MIN_ACCEL_X,max_acceleration_x); 101 | acceleration_x = constrain(acceleration_x,MIN_ACCEL_X,max_acceleration_x); 102 | 103 | acceleration_y = map(abs(speed_y),0,accel_ramp,MIN_ACCEL_Y,max_acceleration_y); 104 | acceleration_y = constrain(acceleration_y,MIN_ACCEL_Y,max_acceleration_y); 105 | 106 | // X AXIS 107 | temp = (long)speed_x*speed_x; 108 | temp = temp/(1800*(long)acceleration_x); // 2000*0.85 = 1700 0.85 is a compensation for deceleration ramp (S-curve) 109 | pos_stop_x = position_x + sign(speed_x)*temp; 110 | if (target_position_x>position_x) // Positive move 111 | { 112 | if (pos_stop_x >= target_position_x) // Start decelerating? 113 | setMotorXSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 114 | else 115 | setMotorXSpeed(target_speed_x,dt); // The aceleration ramp is done inside the setSpeed function 116 | } 117 | else // Negative move 118 | { 119 | if (pos_stop_x <= target_position_x) // Start decelerating? 120 | setMotorXSpeed(0,dt); 121 | else 122 | setMotorXSpeed(-target_speed_x,dt); 123 | } 124 | 125 | // Y AXIS 126 | temp = (long)speed_y*speed_y; 127 | temp = temp/(1800*(long)acceleration_y); 128 | pos_stop_y = position_y + sign(speed_y)*temp; 129 | if (target_position_y>position_y) // Positive move 130 | { 131 | if (pos_stop_y >= target_position_y) // Start decelerating? 132 | setMotorYSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 133 | else 134 | setMotorYSpeed(target_speed_y,dt); // The aceleration ramp is done inside the setSpeed function 135 | } 136 | else // Negative move 137 | { 138 | if (pos_stop_y <= target_position_y) // Start decelerating? 139 | setMotorYSpeed(0,dt); 140 | else 141 | setMotorYSpeed(-target_speed_y,dt); 142 | } 143 | CLR(PORTF,3); // for external timing debug 144 | } 145 | 146 | // Speed could be positive or negative 147 | void setMotorXSpeed(int16_t tspeed, int16_t dt) 148 | { 149 | long timer_period; 150 | int16_t accel; 151 | 152 | // Limit max speed 153 | if (tspeed > MAX_SPEED_X) 154 | tspeed = MAX_SPEED_X; 155 | else if (tspeed < -MAX_SPEED_X) 156 | tspeed = -MAX_SPEED_X; 157 | //Serial.println(tspeed); 158 | 159 | // We limit acceleration => speed ramp 160 | accel = ((long)acceleration_x*dt)/1000; // We divide by 1000 because dt are in microseconds 161 | if (((long)tspeed-speed_x)>accel) // We use long here to avoid overflow on the operation 162 | speed_x += accel; 163 | else if (((long)speed_x-tspeed)>accel) 164 | speed_x -= accel; 165 | else 166 | speed_x = tspeed; 167 | 168 | // Check if we need to change the direction pins 169 | if ((speed_x==0)&&(dir_x!=0)) 170 | dir_x = 0; 171 | else if ((speed_x>0)&&(dir_x!=1)) 172 | { 173 | #ifdef INVERT_X_AXIS 174 | CLR(PORTF,1); // X-DIR 175 | #else 176 | SET(PORTF,1); 177 | #endif 178 | dir_x = 1; 179 | } 180 | else if ((speed_x<0)&&(dir_x!=-1)) 181 | { 182 | #ifdef INVERT_X_AXIS 183 | SET(PORTF,1); 184 | #else 185 | CLR(PORTF,1); 186 | #endif 187 | dir_x = -1; 188 | } 189 | 190 | if (speed_x==0) 191 | timer_period = ZERO_SPEED; 192 | else if (speed_x>0) 193 | timer_period = 2000000/speed_x; // 2Mhz timer 194 | else 195 | timer_period = 2000000/-speed_x; 196 | 197 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 198 | timer_period = ZERO_SPEED; 199 | 200 | OCR1A = timer_period; 201 | // Check if we need to reset the timer... 202 | if (TCNT1 > OCR1A) 203 | TCNT1 = 0; 204 | } 205 | 206 | // Speed could be positive or negative 207 | void setMotorYSpeed(int16_t tspeed,int16_t dt) 208 | { 209 | long timer_period; 210 | int16_t accel; 211 | 212 | // Limit max speed 213 | if (tspeed > MAX_SPEED_Y) 214 | tspeed = MAX_SPEED_Y; 215 | else if (tspeed < -MAX_SPEED_Y) 216 | tspeed = -MAX_SPEED_Y; 217 | //Serial.println(tspeed); 218 | 219 | // We limit acceleration => speed ramp 220 | accel = ((long)acceleration_y*dt)/1000; 221 | if (((long)tspeed-speed_y)>accel) 222 | speed_y += accel; 223 | else if (((long)speed_y-tspeed)>accel) 224 | speed_y -= accel; 225 | else 226 | speed_y = tspeed; 227 | 228 | // Check if we need to change the direction pins 229 | if ((speed_y==0)&&(dir_y!=0)) 230 | dir_y = 0; 231 | else if ((speed_y>0)&&(dir_y!=1)) 232 | { 233 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 234 | CLR(PORTF,7); 235 | #else 236 | SET(PORTF,7); 237 | #endif 238 | 239 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 240 | CLR(PORTL,1); 241 | #else 242 | SET(PORTL,1); 243 | #endif 244 | 245 | dir_y = 1; 246 | } 247 | else if ((speed_y<0)&&(dir_y!=-1)) 248 | { 249 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 250 | SET(PORTF,7); 251 | #else 252 | CLR(PORTF,7); 253 | #endif 254 | 255 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 256 | SET(PORTL,1); 257 | #else 258 | CLR(PORTL,1); 259 | #endif 260 | 261 | dir_y = -1; 262 | } 263 | 264 | if (speed_y==0) 265 | timer_period = ZERO_SPEED; 266 | else if (speed_y>0) 267 | timer_period = 2000000/speed_y; // 2Mhz timer 268 | else 269 | timer_period = 2000000/-speed_y; 270 | 271 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 272 | timer_period = ZERO_SPEED; 273 | 274 | OCR3A = timer_period; 275 | // Check if we need to reset the timer... 276 | if (TCNT3 > OCR3A) 277 | TCNT3 = 0; 278 | } 279 | 280 | // Set speed on each axis in steps/sec 281 | void setSpeedS(int target_sx, int target_sy) 282 | { 283 | target_sx = constrain(target_sx,0,MAX_SPEED_X); 284 | target_sy = constrain(target_sy,0,MAX_SPEED_Y); 285 | target_speed_x = target_sx; 286 | target_speed_y = target_sy; 287 | } 288 | 289 | // set Robot position in mm. 290 | // This function check for valid robot positions values 291 | // Convert from mm units to steps 292 | void setPosition(int target_x_mm_new, int target_y_mm_new) 293 | { 294 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 295 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 296 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 297 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 298 | } 299 | 300 | // set Robot position in mm. 301 | // This function check for valid robot positions values 302 | // Convert from mm units to steps 303 | void setPosition_straight(int target_x_mm_new, int target_y_mm_new) 304 | { 305 | int old_target_position_x; 306 | int old_target_position_y; 307 | int diff_x; 308 | int diff_y; 309 | 310 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 311 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 312 | old_target_position_x = target_position_x; 313 | old_target_position_y = target_position_y; 314 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 315 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 316 | // Speed adjust to draw straight lines 317 | diff_x = myAbs(target_position_x - old_target_position_x); 318 | diff_y = myAbs(target_position_y - old_target_position_y); 319 | if (diff_x > diff_y) // Wich axis should be slower? 320 | { 321 | com_speed_x = target_speed_x; 322 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 323 | setSpeedS(com_speed_x,com_speed_y); 324 | } 325 | else 326 | { 327 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 328 | com_speed_y = target_speed_y; 329 | setSpeedS(com_speed_x,com_speed_y); 330 | } 331 | } 332 | 333 | // set Robot position in 1/10 mm. 334 | // This function check for valid robot positions values 335 | // Convert from 1/10 mm units to steps 336 | // This function moves the robot in a straight line 337 | void setPosition_mm10_straight(int target_x_mm_new, int target_y_mm_new) 338 | { 339 | int old_target_position_x; 340 | int old_target_position_y; 341 | int diff_x; 342 | int diff_y; 343 | 344 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X*10,ROBOT_MAX_X*10); 345 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y*10,ROBOT_MAX_Y*10); 346 | old_target_position_x = target_position_x; 347 | old_target_position_y = target_position_y; 348 | target_position_x = (float)target_x_mm*X_AXIS_STEPS_PER_UNIT/10.0; 349 | target_position_y = (float)target_y_mm*Y_AXIS_STEPS_PER_UNIT/10.0; 350 | // Speed adjust to draw straight lines 351 | diff_x = myAbs(target_position_x - old_target_position_x); 352 | diff_y = myAbs(target_position_y - old_target_position_y); 353 | if (diff_x > diff_y) // Wich axis should be slower? 354 | { 355 | com_speed_x = target_speed_x; 356 | com_speed_y = (float)target_speed_y*(float)diff_y/(float)diff_x; 357 | setSpeedS(com_speed_x,com_speed_y); 358 | } 359 | else 360 | { 361 | com_speed_x = (float)target_speed_x*(float)diff_x/(float)diff_y; 362 | com_speed_y = target_speed_y; 363 | setSpeedS(com_speed_x,com_speed_y); 364 | } 365 | } 366 | 367 | 368 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_paint/AHR_paint.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | // VERION FOR PAINT A GCODE WITH THE ROBOT!!!!!! 3 | 4 | // Author: Jose Julio (@jjdrones) 5 | // Hardware: Arduino MEGA + Ramps 1.4 6 | // Date: 21/01/2014 7 | // Updated: 8 | // Version: 1.01 9 | // Project page : Spanish: English: 10 | // GIT repository: 11 | // License: Open Software GPL License 12 | 13 | // ROBOT and USER configuration parameters 14 | #include "Configuration.h" 15 | #include "Definitions.h" // Variable definitions 16 | 17 | // Array with the Gcode 18 | #include "GCODE.h" 19 | 20 | void setup() 21 | { 22 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 23 | // X MOTOR 24 | // X-STEP: A0 (PF0) 25 | // X-DIR: A1 (PF1) 26 | // X-ENABLE: D38 (PD7) 27 | // Y MOTOR (Y-LEFT) 28 | // Y-STEP: A6 (PF6) 29 | // Y-DIR: A7 (PF7) 30 | // Y-ENABLE: A2 (PF2) 31 | // Z MOTOR (Y-RIGHT) 32 | // Z-STEP: D46 (PL3) 33 | // Z-DIR: D48 (PL1) 34 | // Z-ENABLE: A8 (PK0) 35 | 36 | // STEPPER PINS 37 | // X_AXIS 38 | pinMode(38,OUTPUT); // ENABLE MOTOR 39 | pinMode(A0,OUTPUT); // STEP MOTOR 40 | pinMode(A1,OUTPUT); // DIR MOTOR 41 | // Y_AXIS (Y-LEFT) 42 | pinMode(A2,OUTPUT); // ENABLE MOTOR 43 | pinMode(A6,OUTPUT); // STEP MOTOR 44 | pinMode(A7,OUTPUT); // DIR MOTOR 45 | // Z_AXIS (Y-RIGHT) 46 | pinMode(A8,OUTPUT); // ENABLE MOTOR 47 | pinMode(46,OUTPUT); // STEP MOTOR 48 | pinMode(48,OUTPUT); // DIR MOTOR 49 | 50 | pinMode(A3,OUTPUT); // DEBUG PIN FOR OSCILLOSCOPE TIME MEASURES 51 | 52 | pinMode(19,INPUT); // RX1 Serial Port 1 53 | pinMode(18,OUTPUT); // TX1 54 | 55 | //FANS and LEDS 56 | pinMode(8,OUTPUT); 57 | pinMode(9,OUTPUT); 58 | pinMode(10,OUTPUT); 59 | pinMode(13,OUTPUT); 60 | 61 | // Disable Motors 62 | digitalWrite(38,HIGH); 63 | digitalWrite(A2,HIGH); 64 | digitalWrite(A8,HIGH); 65 | 66 | Serial.begin(115200); 67 | Serial.println("AHR Robot GCODE paint version v1.01"); 68 | Serial.println("Initializing robot..."); 69 | Serial.print("Free Memory: "); 70 | Serial.print(freeRam()); 71 | Serial.println(); 72 | delay(2500); 73 | 74 | //LED blink 75 | for (uint8_t k=0;k<4;k++) 76 | { 77 | digitalWrite(13,HIGH); 78 | delay(300); 79 | digitalWrite(13,LOW); 80 | delay(300); 81 | } 82 | 83 | // We use TIMER 1 for stepper motor X AXIS and Timer 3 for Y AXIS 84 | // STEPPER MOTORS INITIALIZATION 85 | // TIMER1 CTC MODE 86 | TCCR1B &= ~(1<=1000) // 1Khz loop 179 | { 180 | //dt = (timer_value-timer_old)/1000; 181 | timer_old = timer_value; 182 | loop_counter++; 183 | 184 | if ((myAbs(speed_x)<2)&&(myAbs(speed_y)<2)) 185 | stopped_counter++; 186 | else 187 | stopped_counter=0; 188 | 189 | if (stopped_counter>=2) // Move done? 50ms stopped 190 | { 191 | if (gcode_index < gcode_lines) 192 | { 193 | int new_pos_x_mm10 = gcode[gcode_index][0]*3; 194 | int new_pos_y_mm10 = gcode[gcode_index][1]*4 + 000; 195 | Serial.print(gcode_index); 196 | Serial.print(":"); 197 | Serial.print(new_pos_x_mm10); 198 | Serial.print(","); 199 | Serial.print(new_pos_y_mm10); 200 | Serial.print(" "); 201 | Serial.print(position_x); 202 | Serial.print(";"); 203 | Serial.print(position_y); 204 | Serial.print(" "); 205 | 206 | if (gcode_index==0) 207 | { 208 | Serial.print("FIRST MOVE"); 209 | max_speed_x = MAX_SPEED_X; 210 | max_speed_y = MAX_SPEED_Y; 211 | } 212 | else 213 | { 214 | // We use a much lower speed for paint 215 | max_speed_x = SPEED_PAINT; 216 | max_speed_y = SPEED_PAINT; 217 | } 218 | 219 | // Send coordinates to robot in mm/10 units 220 | setPosition_mm10(new_pos_x_mm10,new_pos_y_mm10); 221 | gcode_index++; 222 | Serial.println(); 223 | } 224 | else 225 | { 226 | Serial.print("RETURN HOME "); 227 | com_pos_x = ROBOT_CENTER_X; 228 | com_pos_y = ROBOT_MIN_Y; 229 | setPosition_mm10(com_pos_x*10,com_pos_y*10); 230 | Serial.println(); 231 | } 232 | } 233 | 234 | positionControl(); 235 | } // 1Khz loop 236 | } 237 | 238 | 239 | 240 | 241 | 242 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_paint/Configuration.h: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // THIS VALUES DEPENDS ON THE MOTORS, PULLEYS AND ROBOT CONSTRUCTION 4 | #define MIN_ACCEL_X 50 5 | #define MAX_ACCEL_X 50 //320 // Maximun motor acceleration in (steps/seg2)/1000 6 | #define MIN_ACCEL_Y 50 7 | #define MAX_ACCEL_Y 50 //220 8 | #define MAX_SPEED_X 4000 //25000 // Maximun speed in steps/seg 9 | #define MAX_SPEED_Y 4000 10 | #define SPEED_PAINT 500 11 | 12 | // This is for the Accel ramp implementation (to smooth the intial acceleration), simplified S-profile 13 | #define ACCEL_RAMP_MIN 10 14 | #define ACCEL_RAMP_MAX 10000 15 | 16 | // UNCOMMENT THIS LINES TO INVERT MOTORS 17 | #define INVERT_X_AXIS 1 18 | //#define INVERT_Y_AXIS 1 //Y-LEFT 19 | //#define INVERT_Z_AXIS 1 //Y_RIGHT 20 | 21 | // This depends on the pulley teeth. For 42 teeth GT2 => 19, for 40 teeth GT2 => 20 22 | #define X_AXIS_STEPS_PER_UNIT 19.04 // With 42 teeth GT2 pulley and 1/8 microstepping on drivers 23 | #define Y_AXIS_STEPS_PER_UNIT 19.04 // 200*8 = 1600 steps/rev = 1600/42*2 = 19.047, using 19 is an error of 1mm every 40cm not too much! and we use int operations... 24 | 25 | // Min and Max robot positions in mm 26 | #define ROBOT_MIN_X 60 27 | #define ROBOT_MIN_Y 100 28 | #define ROBOT_MAX_X 540 29 | #define ROBOT_MAX_Y 440 30 | 31 | // All units in milimeters 32 | #define ROBOT_CENTER_X 300 // Center of robot. The table is 600x1000mm, so center is 300,500 33 | #define ROBOT_CENTER_Y 500 34 | 35 | // Initial robot position in mm 36 | // The robot must be at this position at start time 37 | #define ROBOT_INITIAL_POSITION_X 300 38 | #define ROBOT_INITIAL_POSITION_Y 100 39 | 40 | #define POSITION_TOLERANCE 4 // 5 steps 41 | 42 | // PS3 Camera pixels 43 | // NOTE: We are using the camera at 320x240 but we are outputing a 640x480 pixel equivalent position 44 | #define CAM_PIX_WIDTH 640 45 | #define CAM_PIX_HEIGHT 480 46 | #define CAM_PIX_TO_MM 1.4 // Camera scale To calculate, you need to measure two points with known coordinates 47 | 48 | // Utils 49 | #define CLR(x,y) (x&=(~(1< 4 | 5 | // Variable definitions 6 | 7 | // Log and Timer variables 8 | long loop_counter; 9 | long timer_old; 10 | long timer_packet_old; 11 | long timer_value; 12 | int debug_counter; 13 | 14 | uint32_t micros_old; 15 | 16 | // We have 2 axis => 2 motor controls 0=X axis 1=Y axis (Y AXIS HAS 2 MOTORS Left and Right) 17 | int16_t speed_m[2]; // Actual speed of motors 18 | uint8_t dir_m[2]; // Actual direction of steppers motors 19 | 20 | uint16_t counter_m[2]; // counters for periods 21 | uint16_t period_m[2][8]; // Eight subperiods 22 | uint8_t period_m_index[2]; // index for subperiods 23 | 24 | // kinematic variables 25 | // position, speed and acceleration are in step units 26 | volatile int16_t position_x; // This variables are modified inside the Timer interrupts 27 | volatile int16_t position_y; 28 | 29 | int16_t speed_x; 30 | int16_t speed_y; 31 | int16_t max_speed_x; 32 | int16_t max_speed_y; 33 | 34 | int8_t dir_x; //(dir=1 positive, dir=-1 negative) 35 | int8_t dir_y; 36 | int16_t target_position_x; 37 | int16_t target_position_y; 38 | int16_t target_speed_x; 39 | int16_t target_speed_y; 40 | int16_t max_acceleration_x = MAX_ACCEL_X; // default maximun acceleration 41 | int16_t max_acceleration_y = MAX_ACCEL_Y; 42 | int16_t acceleration_x = MAX_ACCEL_X; 43 | int16_t acceleration_y = MAX_ACCEL_Y; 44 | int16_t accel_ramp = ACCEL_RAMP_MIN; 45 | 46 | int16_t pos_stop_x; 47 | int16_t pos_stop_y; 48 | 49 | uint16_t com_pos_x; 50 | uint16_t com_pos_y; 51 | uint16_t com_speed_x; 52 | uint16_t com_speed_y; 53 | uint16_t target_x_mm; 54 | uint16_t target_y_mm; 55 | int16_t user_speed_x; 56 | int16_t user_speed_y; 57 | int16_t filt_user_speed_x; 58 | int16_t filt_user_speed_y; 59 | 60 | 61 | // Some util functions... 62 | int freeRam () { 63 | extern int __heap_start, *__brkval; 64 | int v; 65 | return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 66 | } 67 | 68 | int16_t myAbs(int16_t param) 69 | { 70 | if (param<0) 71 | return -param; 72 | else 73 | return param; 74 | } 75 | 76 | int sign(int val) 77 | { 78 | if (val<0) 79 | return(-1); 80 | else 81 | return(1); 82 | } 83 | 84 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_paint/GCODE.h: -------------------------------------------------------------------------------- 1 | // This array is generated from a gcode file 2 | // using a python code to extract coordinates 3 | 4 | /* 5 | // Gode array in 1/10mm units 6 | int gcode_index=0; 7 | int gcode_lines=529; //529 8 | 9 | int gcode[529][2]={ 10 | {614,774}, 11 | {628,764}, 12 | {636,761}, 13 | {654,759}, 14 | {674,762}, 15 | {683,767}, 16 | {691,773}, 17 | {698,781}, 18 | {704,791}, 19 | {712,814}, 20 | {698,819}, 21 | {692,801}, 22 | {688,793}, 23 | {676,781}, 24 | {662,775}, 25 | {643,775}, 26 | {633,779}, 27 | {625,785}, 28 | {617,793}, 29 | {612,804}, 30 | {608,817}, 31 | {605,832}, 32 | {604,849}, 33 | {606,872}, 34 | {611,890}, 35 | {618,904}, 36 | {629,914}, 37 | {641,920}, 38 | {655,922}, 39 | {669,920}, 40 | {681,912}, 41 | {690,900}, 42 | {695,886}, 43 | {710,890}, 44 | {702,909}, 45 | {690,925}, 46 | {674,934}, 47 | {661,937}, 48 | {656,937}, 49 | {638,935}, 50 | {630,932}, 51 | {615,922}, 52 | {609,915}, 53 | {598,898}, 54 | {594,887}, 55 | {592,876}, 56 | {589,849}, 57 | {592,822}, 58 | {594,810}, 59 | {598,799}, 60 | {608,781}, 61 | {614,774}, 62 | {747,794}, 63 | {749,785}, 64 | {753,777}, 65 | {761,773}, 66 | {771,771}, 67 | {781,773}, 68 | {789,777}, 69 | {797,783}, 70 | {802,791}, 71 | {805,802}, 72 | {806,827}, 73 | {796,824}, 74 | {758,813}, 75 | {753,810}, 76 | {748,800}, 77 | {747,794}, 78 | {743,791}, 79 | {735,779}, 80 | {738,773}, 81 | {747,764}, 82 | {759,760}, 83 | {776,760}, 84 | {786,763}, 85 | {795,769}, 86 | {807,780}, 87 | {811,762}, 88 | {824,762}, 89 | {820,775}, 90 | {819,842}, 91 | {817,868}, 92 | {813,876}, 93 | {806,882}, 94 | {796,887}, 95 | {781,888}, 96 | {763,886}, 97 | {750,879}, 98 | {741,868}, 99 | {737,853}, 100 | {749,851}, 101 | {753,861}, 102 | {759,869}, 103 | {767,874}, 104 | {779,875}, 105 | {792,873}, 106 | {801,867}, 107 | {805,858}, 108 | {806,839}, 109 | {791,835}, 110 | {758,828}, 111 | {749,824}, 112 | {742,819}, 113 | {737,812}, 114 | {734,803}, 115 | {733,794}, 116 | {852,762}, 117 | {865,762}, 118 | {865,830}, 119 | {867,850}, 120 | {873,864}, 121 | {883,872}, 122 | {895,874}, 123 | {902,873}, 124 | {913,866}, 125 | {917,859}, 126 | {919,850}, 127 | {919,762}, 128 | {932,762}, 129 | {932,838}, 130 | {930,864}, 131 | {928,871}, 132 | {919,882}, 133 | {914,885}, 134 | {903,888}, 135 | {899,888}, 136 | {889,887}, 137 | {881,883}, 138 | {874,877}, 139 | {864,863}, 140 | {864,863}, 141 | {863,885}, 142 | {852,885}, 143 | {852,763}, 144 | {1019,728}, 145 | {1020,712}, 146 | {1030,710}, 147 | {1036,711}, 148 | {1041,713}, 149 | {1051,725}, 150 | {1063,758}, 151 | {1102,885}, 152 | {1089,885}, 153 | {1057,778}, 154 | {1057,778}, 155 | {1026,885}, 156 | {1012,885}, 157 | {1051,759}, 158 | {1044,737}, 159 | {1037,729}, 160 | {1026,726}, 161 | {1019,727}, 162 | {1139,785}, 163 | {1149,775}, 164 | {1163,772}, 165 | {1176,775}, 166 | {1187,785}, 167 | {1194,801}, 168 | {1196,824}, 169 | {1194,846}, 170 | {1186,862}, 171 | {1175,872}, 172 | {1162,875}, 173 | {1158,875}, 174 | {1149,872}, 175 | {1143,868}, 176 | {1134,855}, 177 | {1131,846}, 178 | {1129,824}, 179 | {1131,801}, 180 | {1138,786}, 181 | {1137,781}, 182 | {1136,768}, 183 | {1144,763}, 184 | {1153,760}, 185 | {1162,759}, 186 | {1172,760}, 187 | {1181,763}, 188 | {1189,768}, 189 | {1196,775}, 190 | {1202,784}, 191 | {1206,795}, 192 | {1209,825}, 193 | {1206,852}, 194 | {1202,863}, 195 | {1196,872}, 196 | {1189,879}, 197 | {1181,884}, 198 | {1168,888}, 199 | {1162,888}, 200 | {1153,887}, 201 | {1144,884}, 202 | {1136,879}, 203 | {1129,872}, 204 | {1123,863}, 205 | {1119,852}, 206 | {1116,824}, 207 | {1119,795}, 208 | {1123,784}, 209 | {1129,776}, 210 | {1235,793}, 211 | {1238,781}, 212 | {1242,772}, 213 | {1248,765}, 214 | {1257,761}, 215 | {1268,759}, 216 | {1278,760}, 217 | {1286,764}, 218 | {1293,770}, 219 | {1303,785}, 220 | {1303,785}, 221 | {1304,762}, 222 | {1315,762}, 223 | {1315,885}, 224 | {1302,885}, 225 | {1302,819}, 226 | {1300,798}, 227 | {1298,790}, 228 | {1290,779}, 229 | {1284,776}, 230 | {1272,773}, 231 | {1264,774}, 232 | {1258,777}, 233 | {1253,783}, 234 | {1250,789}, 235 | {1248,817}, 236 | {1248,885}, 237 | {1235,885}, 238 | {1235,793}, 239 | {1349,637}, 240 | {1335,628}, 241 | {1326,613}, 242 | {1321,593}, 243 | {1335,591}, 244 | {1338,606}, 245 | {1345,618}, 246 | {1355,625}, 247 | {1366,627}, 248 | {1377,624}, 249 | {1387,617}, 250 | {1394,606}, 251 | {1396,592}, 252 | {1393,579}, 253 | {1364,541}, 254 | {1360,528}, 255 | {1359,508}, 256 | {1371,508}, 257 | {1374,531}, 258 | {1379,540}, 259 | {1399,563}, 260 | {1406,574}, 261 | {1409,583}, 262 | {1409,604}, 263 | {1403,620}, 264 | {1390,633}, 265 | {1383,637}, 266 | {1371,639}, 267 | {1366,640}, 268 | {1350,637}, 269 | {1277,565}, 270 | {1267,575}, 271 | {1253,578}, 272 | {1241,575}, 273 | {1231,567}, 274 | {1224,555}, 275 | {1221,536}, 276 | {1285,536}, 277 | {1282,554}, 278 | {1277,564}, 279 | {1279,569}, 280 | {1279,582}, 281 | {1271,587}, 282 | {1259,590}, 283 | {1253,591}, 284 | {1244,590}, 285 | {1235,587}, 286 | {1227,582}, 287 | {1220,574}, 288 | {1214,565}, 289 | {1210,554}, 290 | {1207,525}, 291 | {1210,498}, 292 | {1214,487}, 293 | {1220,478}, 294 | {1227,471}, 295 | {1235,466}, 296 | {1244,463}, 297 | {1254,462}, 298 | {1270,464}, 299 | {1282,472}, 300 | {1292,484}, 301 | {1297,499}, 302 | {1284,501}, 303 | {1280,491}, 304 | {1273,482}, 305 | {1264,477}, 306 | {1254,475}, 307 | {1245,477}, 308 | {1236,482}, 309 | {1226,493}, 310 | {1223,501}, 311 | {1220,523}, 312 | {1298,523}, 313 | {1298,542}, 314 | {1295,555}, 315 | {1291,566}, 316 | {1286,574}, 317 | {1358,485}, 318 | {1358,465}, 319 | {1373,465}, 320 | {1373,485}, 321 | {1358,485}, 322 | {1182,465}, 323 | {1182,550}, 324 | {1180,569}, 325 | {1173,582}, 326 | {1167,587}, 327 | {1158,590}, 328 | {1143,590}, 329 | {1135,586}, 330 | {1127,579}, 331 | {1118,566}, 332 | {1113,579}, 333 | {1107,586}, 334 | {1095,590}, 335 | {1089,591}, 336 | {1080,590}, 337 | {1072,586}, 338 | {1065,580}, 339 | {1056,566}, 340 | {1056,566}, 341 | {1055,588}, 342 | {1044,588}, 343 | {1044,465}, 344 | {1057,465}, 345 | {1057,529}, 346 | {1059,551}, 347 | {1064,566}, 348 | {1074,574}, 349 | {1086,577}, 350 | {1094,575}, 351 | {1101,570}, 352 | {1105,560}, 353 | {1106,546}, 354 | {1106,465}, 355 | {1119,465}, 356 | {1119,537}, 357 | {1121,554}, 358 | {1127,567}, 359 | {1136,575}, 360 | {1148,577}, 361 | {1159,574}, 362 | {1164,569}, 363 | {1168,555}, 364 | {1169,465}, 365 | {1181,465}, 366 | {968,465}, 367 | {966,479}, 368 | {954,480}, 369 | {950,484}, 370 | {948,498}, 371 | {948,576}, 372 | {966,576}, 373 | {966,588}, 374 | {948,588}, 375 | {948,631}, 376 | {935,621}, 377 | {935,588}, 378 | {922,588}, 379 | {922,576}, 380 | {935,576}, 381 | {935,500}, 382 | {937,475}, 383 | {940,470}, 384 | {944,466}, 385 | {950,464}, 386 | {957,463}, 387 | {967,464}, 388 | {881,494}, 389 | {884,504}, 390 | {885,530}, 391 | {875,526}, 392 | {837,516}, 393 | {832,513}, 394 | {827,503}, 395 | {826,496}, 396 | {828,487}, 397 | {833,480}, 398 | {840,475}, 399 | {850,474}, 400 | {860,475}, 401 | {869,479}, 402 | {876,486}, 403 | {881,493}, 404 | {886,494}, 405 | {890,465}, 406 | {903,465}, 407 | {900,477}, 408 | {898,545}, 409 | {896,571}, 410 | {892,579}, 411 | {885,585}, 412 | {875,589}, 413 | {861,591}, 414 | {842,589}, 415 | {829,582}, 416 | {821,571}, 417 | {816,556}, 418 | {829,554}, 419 | {832,564}, 420 | {838,572}, 421 | {846,577}, 422 | {858,578}, 423 | {871,576}, 424 | {880,569}, 425 | {884,561}, 426 | {885,542}, 427 | {870,537}, 428 | {838,531}, 429 | {828,527}, 430 | {822,522}, 431 | {816,515}, 432 | {813,506}, 433 | {812,496}, 434 | {814,482}, 435 | {817,476}, 436 | {826,467}, 437 | {838,462}, 438 | {855,463}, 439 | {865,466}, 440 | {874,472}, 441 | {885,482}, 442 | {778,536}, 443 | {775,554}, 444 | {770,565}, 445 | {759,575}, 446 | {746,578}, 447 | {734,575}, 448 | {724,567}, 449 | {717,555}, 450 | {714,536}, 451 | {777,536}, 452 | {783,537}, 453 | {788,555}, 454 | {784,566}, 455 | {778,575}, 456 | {771,582}, 457 | {764,587}, 458 | {751,590}, 459 | {746,591}, 460 | {737,590}, 461 | {728,587}, 462 | {720,582}, 463 | {713,574}, 464 | {707,565}, 465 | {703,554}, 466 | {699,525}, 467 | {703,498}, 468 | {707,487}, 469 | {713,478}, 470 | {720,471}, 471 | {728,466}, 472 | {737,463}, 473 | {747,462}, 474 | {762,464}, 475 | {775,472}, 476 | {785,484}, 477 | {790,499}, 478 | {776,501}, 479 | {772,491}, 480 | {766,482}, 481 | {757,477}, 482 | {747,475}, 483 | {740,476}, 484 | {728,482}, 485 | {719,493}, 486 | {716,501}, 487 | {713,523}, 488 | {791,523}, 489 | {791,541}, 490 | {666,527}, 491 | {663,549}, 492 | {657,565}, 493 | {647,575}, 494 | {635,578}, 495 | {631,577}, 496 | {623,574}, 497 | {613,565}, 498 | {609,557}, 499 | {606,549}, 500 | {604,527}, 501 | {605,510}, 502 | {608,497}, 503 | {612,487}, 504 | {619,481}, 505 | {627,476}, 506 | {635,475}, 507 | {647,478}, 508 | {657,488}, 509 | {663,504}, 510 | {666,526}, 511 | {670,529}, 512 | {678,547}, 513 | {673,563}, 514 | {666,575}, 515 | {658,584}, 516 | {648,589}, 517 | {637,591}, 518 | {629,590}, 519 | {622,587}, 520 | {606,571}, 521 | {606,637}, 522 | {593,637}, 523 | {593,465}, 524 | {604,465}, 525 | {605,485}, 526 | {605,485}, 527 | {614,472}, 528 | {621,466}, 529 | {628,463}, 530 | {636,462}, 531 | {644,463}, 532 | {652,466}, 533 | {659,471}, 534 | {666,479}, 535 | {672,488}, 536 | {676,499}, 537 | {678,512}, 538 | {679,527} 539 | }; 540 | 541 | */ 542 | 543 | // WANNA PLAY 544 | // Gode array in 1/10mm units 545 | int gcode_index=0; 546 | int gcode_lines=358; 547 | 548 | int gcode[358][2]={ 549 | {1637,677}, 550 | {1653,677}, 551 | {1654,693}, 552 | {1657,705}, 553 | {1663,715}, 554 | {1687,744}, 555 | {1694,756}, 556 | {1698,768}, 557 | {1699,781}, 558 | {1698,792}, 559 | {1695,803}, 560 | {1691,812}, 561 | {1684,821}, 562 | {1676,828}, 563 | {1667,832}, 564 | {1653,836}, 565 | {1646,836}, 566 | {1635,835}, 567 | {1626,833}, 568 | {1617,828}, 569 | {1609,822}, 570 | {1603,814}, 571 | {1598,803}, 572 | {1592,779}, 573 | {1609,777}, 574 | {1613,795}, 575 | {1621,809}, 576 | {1633,817}, 577 | {1646,820}, 578 | {1660,817}, 579 | {1671,808}, 580 | {1679,795}, 581 | {1682,779}, 582 | {1679,762}, 583 | {1644,717}, 584 | {1639,702}, 585 | {1637,678}, 586 | {1636,650}, 587 | {1636,625}, 588 | {1655,625}, 589 | {1655,650}, 590 | {1636,650}, 591 | {1527,621}, 592 | {1573,774}, 593 | {1557,774}, 594 | {1519,646}, 595 | {1519,646}, 596 | {1482,774}, 597 | {1464,774}, 598 | {1512,622}, 599 | {1503,596}, 600 | {1495,586}, 601 | {1482,583}, 602 | {1473,585}, 603 | {1474,565}, 604 | {1486,563}, 605 | {1494,564}, 606 | {1500,567}, 607 | {1506,572}, 608 | {1512,581}, 609 | {1526,620}, 610 | {1416,660}, 611 | {1419,673}, 612 | {1421,703}, 613 | {1408,699}, 614 | {1363,687}, 615 | {1358,683}, 616 | {1351,671}, 617 | {1351,664}, 618 | {1352,653}, 619 | {1358,644}, 620 | {1367,639}, 621 | {1379,637}, 622 | {1390,639}, 623 | {1401,644}, 624 | {1409,651}, 625 | {1415,660}, 626 | {1420,661}, 627 | {1426,625}, 628 | {1443,625}, 629 | {1439,641}, 630 | {1437,722}, 631 | {1434,753}, 632 | {1430,763}, 633 | {1421,771}, 634 | {1408,776}, 635 | {1391,778}, 636 | {1369,775}, 637 | {1354,767}, 638 | {1343,753}, 639 | {1337,735}, 640 | {1353,732}, 641 | {1357,745}, 642 | {1364,754}, 643 | {1374,759}, 644 | {1388,761}, 645 | {1403,759}, 646 | {1414,751}, 647 | {1419,741}, 648 | {1421,719}, 649 | {1403,713}, 650 | {1364,706}, 651 | {1352,701}, 652 | {1344,694}, 653 | {1338,686}, 654 | {1334,675}, 655 | {1333,663}, 656 | {1336,646}, 657 | {1339,639}, 658 | {1350,628}, 659 | {1365,622}, 660 | {1373,622}, 661 | {1385,623}, 662 | {1396,627}, 663 | {1407,634}, 664 | {1421,646}, 665 | {1302,625}, 666 | {1302,833}, 667 | {1285,833}, 668 | {1285,625}, 669 | {1301,625}, 670 | {1228,654}, 671 | {1236,673}, 672 | {1238,701}, 673 | {1236,727}, 674 | {1228,746}, 675 | {1216,758}, 676 | {1202,762}, 677 | {1197,761}, 678 | {1188,758}, 679 | {1176,745}, 680 | {1171,736}, 681 | {1168,725}, 682 | {1165,699}, 683 | {1168,672}, 684 | {1176,653}, 685 | {1188,642}, 686 | {1202,638}, 687 | {1216,642}, 688 | {1227,653}, 689 | {1233,654}, 690 | {1246,653}, 691 | {1251,667}, 692 | {1254,683}, 693 | {1255,701}, 694 | {1254,724}, 695 | {1248,743}, 696 | {1240,759}, 697 | {1230,769}, 698 | {1218,776}, 699 | {1204,778}, 700 | {1194,776}, 701 | {1185,772}, 702 | {1177,766}, 703 | {1167,750}, 704 | {1166,750}, 705 | {1166,774}, 706 | {1151,774}, 707 | {1151,566}, 708 | {1167,566}, 709 | {1167,645}, 710 | {1178,632}, 711 | {1186,626}, 712 | {1194,623}, 713 | {1203,622}, 714 | {1213,623}, 715 | {1225,629}, 716 | {1239,642}, 717 | {1027,673}, 718 | {1028,703}, 719 | {1016,699}, 720 | {971,687}, 721 | {965,683}, 722 | {959,671}, 723 | {958,664}, 724 | {960,653}, 725 | {966,644}, 726 | {975,639}, 727 | {986,637}, 728 | {998,639}, 729 | {1008,644}, 730 | {1017,651}, 731 | {1023,660}, 732 | {1027,673}, 733 | {1031,675}, 734 | {1029,646}, 735 | {1034,625}, 736 | {1051,625}, 737 | {1046,641}, 738 | {1045,722}, 739 | {1042,753}, 740 | {1037,763}, 741 | {1029,771}, 742 | {1016,776}, 743 | {999,778}, 744 | {977,775}, 745 | {961,767}, 746 | {951,753}, 747 | {945,735}, 748 | {961,732}, 749 | {965,745}, 750 | {972,754}, 751 | {982,759}, 752 | {996,761}, 753 | {1011,759}, 754 | {1022,751}, 755 | {1027,741}, 756 | {1028,719}, 757 | {1011,713}, 758 | {971,706}, 759 | {960,701}, 760 | {952,694}, 761 | {946,686}, 762 | {942,675}, 763 | {940,663}, 764 | {943,646}, 765 | {947,639}, 766 | {957,628}, 767 | {972,622}, 768 | {981,622}, 769 | {993,623}, 770 | {1004,627}, 771 | {1015,634}, 772 | {1028,646}, 773 | {910,625}, 774 | {910,717}, 775 | {908,748}, 776 | {904,757}, 777 | {900,764}, 778 | {894,770}, 779 | {887,774}, 780 | {875,777}, 781 | {869,778}, 782 | {858,776}, 783 | {848,772}, 784 | {839,764}, 785 | {828,749}, 786 | {828,749}, 787 | {827,774}, 788 | {812,774}, 789 | {812,625}, 790 | {829,625}, 791 | {829,707}, 792 | {831,731}, 793 | {839,748}, 794 | {850,757}, 795 | {865,760}, 796 | {873,759}, 797 | {881,755}, 798 | {887,750}, 799 | {891,742}, 800 | {893,731}, 801 | {894,625}, 802 | {909,625}, 803 | {775,625}, 804 | {775,717}, 805 | {772,748}, 806 | {769,757}, 807 | {765,764}, 808 | {759,770}, 809 | {752,774}, 810 | {740,777}, 811 | {734,778}, 812 | {722,776}, 813 | {712,772}, 814 | {704,764}, 815 | {693,749}, 816 | {692,749}, 817 | {692,774}, 818 | {677,774}, 819 | {677,625}, 820 | {694,625}, 821 | {694,707}, 822 | {696,731}, 823 | {703,748}, 824 | {715,757}, 825 | {730,760}, 826 | {738,759}, 827 | {745,755}, 828 | {751,750}, 829 | {755,742}, 830 | {758,731}, 831 | {758,625}, 832 | {774,625}, 833 | {621,673}, 834 | {622,703}, 835 | {610,699}, 836 | {565,687}, 837 | {559,683}, 838 | {553,671}, 839 | {552,664}, 840 | {554,653}, 841 | {560,644}, 842 | {569,639}, 843 | {580,637}, 844 | {592,639}, 845 | {603,644}, 846 | {611,651}, 847 | {617,660}, 848 | {621,673}, 849 | {625,675}, 850 | {623,646}, 851 | {628,625}, 852 | {645,625}, 853 | {640,641}, 854 | {639,722}, 855 | {636,753}, 856 | {631,763}, 857 | {623,771}, 858 | {610,776}, 859 | {593,778}, 860 | {571,775}, 861 | {555,767}, 862 | {545,753}, 863 | {539,735}, 864 | {555,732}, 865 | {559,745}, 866 | {566,754}, 867 | {576,759}, 868 | {590,761}, 869 | {605,759}, 870 | {616,751}, 871 | {621,741}, 872 | {622,719}, 873 | {605,713}, 874 | {565,706}, 875 | {554,701}, 876 | {546,694}, 877 | {540,686}, 878 | {536,675}, 879 | {534,663}, 880 | {537,646}, 881 | {541,639}, 882 | {551,628}, 883 | {566,622}, 884 | {575,622}, 885 | {587,623}, 886 | {598,627}, 887 | {609,634}, 888 | {622,646}, 889 | {472,625}, 890 | {518,833}, 891 | {500,833}, 892 | {465,653}, 893 | {460,653}, 894 | {446,727}, 895 | {422,833}, 896 | {398,833}, 897 | {357,653}, 898 | {352,653}, 899 | {319,833}, 900 | {300,833}, 901 | {346,625}, 902 | {365,625}, 903 | {409,815}, 904 | {410,815}, 905 | {453,625}, 906 | {471,625} 907 | }; 908 | -------------------------------------------------------------------------------- /Arduino/Utils/AHR_paint/Steppers.ino: -------------------------------------------------------------------------------- 1 | // AHR AIR HOCKEY ROBOT PROJECT 2 | 3 | // STEPPERS MOTOR CONTROL 4 | // SPEED, ACCELERATION AND POSITION CONTROL using Arduino 16 bit Timer interrupts 5 | 6 | // STEPPER MOTOR PINS (SAME AS RAMPS 1.4) 7 | // X MOTOR 8 | // X-STEP: A0 (PF0) 9 | // X-DIR: A1 (PF1) 10 | // X-ENABLE: D38 (PD7) 11 | // Y MOTOR (Y-LEFT) 12 | // Y-STEP: A6 (PF6) 13 | // Y-DIR: A7 (PF7) 14 | // Y-ENABLE: A2 (PF2) 15 | // Z MOTOR (Y-RIGHT) 16 | // Z-STEP: D46 (PL3) 17 | // Z-DIR: D48 (PL1) 18 | // Z-ENABLE: A8 (PK0) 19 | // Y and Z motors controls the Y axis of the robot (same output) 20 | 21 | // We control the speed of the motors with interrupts (Timer1 and Timer3) tested up to 25Khz. 22 | // The position of the motor is controlled at 1Khz (in the main loop) 23 | 24 | 25 | // TIMER 1 : STEPPER MOTOR SPEED CONTROL X-AXIS 26 | ISR(TIMER1_COMPA_vect) 27 | { 28 | if (dir_x==0) 29 | return; 30 | 31 | SET(PORTF,0); // STEP X-AXIS 32 | position_x += dir_x; 33 | __asm__ __volatile__ ( 34 | "nop" "\n\t" 35 | "nop"); // Wait 2 cycles. With the other instruction and this we ensure a more than 1 microsenconds step pulse 36 | CLR(PORTF,0); 37 | } 38 | 39 | // TIMER 3 : STEPPER MOTOR SPEED CONTROL Y-AXIS (2 Motors, left and right) 40 | ISR(TIMER3_COMPA_vect) 41 | { 42 | if (dir_y==0) 43 | return; 44 | 45 | SET(PORTF,6); // STEP Y-AXIS (Y-left) 46 | SET(PORTL,3); // STEP Z-AXIS (Y-right) 47 | position_y += dir_y; 48 | // We dont need to wait more, this pulse is around 1.3us (measured with the oscilloscope) 49 | CLR(PORTF,6); 50 | CLR(PORTL,3); 51 | } 52 | 53 | // We use a ramp for acceleration and deceleration 54 | // To calculate the point we should start to decelerate we use this formula: 55 | // stop_position = actual_posicion + (actual_speed*actual_speed)/(2*max_deceleration) 56 | // Beacuase we are using a S-profile we apply a correction factor to the above formula (0.85) 57 | // Input parameters: 58 | // target_position_x 59 | // target_speed_x 60 | // max_acceleration_x 61 | 62 | void positionControl() 63 | { 64 | //int16_t pos_stop; 65 | int32_t temp; 66 | uint32_t timer; 67 | int16_t dt; 68 | 69 | SET(PORTF,3); // for external timing debug 70 | timer = micros(); 71 | // dt = delta time in microseconds... 72 | dt = constrain(timer-micros_old,0,2000); // Limit dt (it should be around 1000 most times) 73 | //Serial.println(dt); 74 | micros_old = timer; 75 | 76 | // We use an acceleration ramp to imitate an S-curve profile at the begining and end (depend on speed) 77 | //acceleration_x = map(abs(speed_x),0,accel_ramp,MIN_ACCEL_X,max_acceleration_x); 78 | //acceleration_x = constrain(acceleration_x,MIN_ACCEL_X,max_acceleration_x); 79 | 80 | //acceleration_y = map(abs(speed_y),0,accel_ramp,MIN_ACCEL_Y,max_acceleration_y); 81 | //acceleration_y = constrain(acceleration_y,MIN_ACCEL_Y,max_acceleration_y); 82 | 83 | acceleration_x = max_acceleration_x; 84 | acceleration_y = max_acceleration_y; 85 | 86 | // X AXIS 87 | temp = (long)speed_x*speed_x; 88 | temp = temp/(2000*(long)acceleration_x); 89 | pos_stop_x = position_x + sign(speed_x)*temp; 90 | if (target_position_x>position_x) // Positive move 91 | { 92 | if (pos_stop_x >= target_position_x) // Start decelerating? 93 | setMotorXSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 94 | else 95 | setMotorXSpeed(target_speed_x,dt); // The aceleration ramp is done inside the setSpeed function 96 | } 97 | else // Negative move 98 | { 99 | if (pos_stop_x <= target_position_x) // Start decelerating? 100 | setMotorXSpeed(0,dt); 101 | else 102 | setMotorXSpeed(-target_speed_x,dt); 103 | } 104 | 105 | // Y AXIS 106 | temp = (long)speed_y*speed_y; 107 | temp = temp/(2000*(long)acceleration_y); 108 | pos_stop_y = position_y + sign(speed_y)*temp; 109 | if (target_position_y>position_y) // Positive move 110 | { 111 | if (pos_stop_y >= target_position_y) // Start decelerating? 112 | setMotorYSpeed(0,dt); // The deceleration ramp is done inside the setSpeed function 113 | else 114 | setMotorYSpeed(target_speed_y,dt); // The aceleration ramp is done inside the setSpeed function 115 | } 116 | else // Negative move 117 | { 118 | if (pos_stop_y <= target_position_y) // Start decelerating? 119 | setMotorYSpeed(0,dt); 120 | else 121 | setMotorYSpeed(-target_speed_y,dt); 122 | } 123 | CLR(PORTF,3); // for external timing debug 124 | } 125 | 126 | // Speed could be positive or negative 127 | void setMotorXSpeed(int16_t tspeed, int16_t dt) 128 | { 129 | long timer_period; 130 | int16_t accel; 131 | 132 | // Limit max speed 133 | if (tspeed > MAX_SPEED_X) 134 | tspeed = MAX_SPEED_X; 135 | else if (tspeed < -MAX_SPEED_X) 136 | tspeed = -MAX_SPEED_X; 137 | //Serial.println(tspeed); 138 | 139 | // We limit acceleration => speed ramp 140 | accel = ((long)acceleration_x*dt)/1000; // We divide by 1000 because dt are in microseconds 141 | if (((long)tspeed-speed_x)>accel) // We use long here to avoid overflow on the operation 142 | speed_x += accel; 143 | else if (((long)speed_x-tspeed)>accel) 144 | speed_x -= accel; 145 | else 146 | speed_x = tspeed; 147 | 148 | // Check if we need to change the direction pins 149 | if ((speed_x==0)&&(dir_x!=0)) 150 | dir_x = 0; 151 | else if ((speed_x>0)&&(dir_x!=1)) 152 | { 153 | #ifdef INVERT_X_AXIS 154 | CLR(PORTF,1); // X-DIR 155 | #else 156 | SET(PORTF,1); 157 | #endif 158 | dir_x = 1; 159 | } 160 | else if ((speed_x<0)&&(dir_x!=-1)) 161 | { 162 | #ifdef INVERT_X_AXIS 163 | SET(PORTF,1); 164 | #else 165 | CLR(PORTF,1); 166 | #endif 167 | dir_x = -1; 168 | } 169 | 170 | if (speed_x==0) 171 | timer_period = ZERO_SPEED; 172 | else if (speed_x>0) 173 | timer_period = 2000000/speed_x; // 2Mhz timer 174 | else 175 | timer_period = 2000000/-speed_x; 176 | 177 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 178 | timer_period = ZERO_SPEED; 179 | 180 | OCR1A = timer_period; 181 | // Check if we need to reset the timer... 182 | if (TCNT1 > OCR1A) 183 | TCNT1 = 0; 184 | } 185 | 186 | // Speed could be positive or negative 187 | void setMotorYSpeed(int16_t tspeed,int16_t dt) 188 | { 189 | long timer_period; 190 | int16_t accel; 191 | 192 | // Limit max speed 193 | if (tspeed > MAX_SPEED_Y) 194 | tspeed = MAX_SPEED_Y; 195 | else if (tspeed < -MAX_SPEED_Y) 196 | tspeed = -MAX_SPEED_Y; 197 | //Serial.println(tspeed); 198 | 199 | // We limit acceleration => speed ramp 200 | accel = ((long)acceleration_y*dt)/1000; 201 | if (((long)tspeed-speed_y)>accel) 202 | speed_y += accel; 203 | else if (((long)speed_y-tspeed)>accel) 204 | speed_y -= accel; 205 | else 206 | speed_y = tspeed; 207 | 208 | // Check if we need to change the direction pins 209 | if ((speed_y==0)&&(dir_y!=0)) 210 | dir_y = 0; 211 | else if ((speed_y>0)&&(dir_y!=1)) 212 | { 213 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 214 | CLR(PORTF,7); 215 | #else 216 | SET(PORTF,7); 217 | #endif 218 | 219 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 220 | CLR(PORTL,1); 221 | #else 222 | SET(PORTL,1); 223 | #endif 224 | 225 | dir_y = 1; 226 | } 227 | else if ((speed_y<0)&&(dir_y!=-1)) 228 | { 229 | #ifdef INVERT_Y_AXIS // Y-DIR (Y-left) 230 | SET(PORTF,7); 231 | #else 232 | CLR(PORTF,7); 233 | #endif 234 | 235 | #ifdef INVERT_Z_AXIS // Z-DIR (Y-right) 236 | SET(PORTL,1); 237 | #else 238 | CLR(PORTL,1); 239 | #endif 240 | 241 | dir_y = -1; 242 | } 243 | 244 | if (speed_y==0) 245 | timer_period = ZERO_SPEED; 246 | else if (speed_y>0) 247 | timer_period = 2000000/speed_y; // 2Mhz timer 248 | else 249 | timer_period = 2000000/-speed_y; 250 | 251 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 252 | timer_period = ZERO_SPEED; 253 | 254 | OCR3A = timer_period; 255 | // Check if we need to reset the timer... 256 | if (TCNT3 > OCR3A) 257 | TCNT3 = 0; 258 | } 259 | 260 | // set Robot position in mm. 261 | // This function check for valid robot positions values 262 | // Convert from mm units to steps 263 | void setPosition(int target_x_mm_new, int target_y_mm_new) 264 | { 265 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X,ROBOT_MAX_X); 266 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y,ROBOT_MAX_Y); 267 | target_position_x = target_x_mm*X_AXIS_STEPS_PER_UNIT; 268 | target_position_y = target_y_mm*Y_AXIS_STEPS_PER_UNIT; 269 | } 270 | 271 | // set Robot position in 1/10 mm. 272 | // This function check for valid robot positions values 273 | // Convert from 1/10 mm units to steps 274 | // This function moves the robot in a straight line 275 | void setPosition_mm10(int target_x_mm_new, int target_y_mm_new) 276 | { 277 | int old_target_position_x; 278 | int old_target_position_y; 279 | int diff_x; 280 | int diff_y; 281 | 282 | target_x_mm = constrain(target_x_mm_new,ROBOT_MIN_X*10,ROBOT_MAX_X*10); 283 | target_y_mm = constrain(target_y_mm_new,ROBOT_MIN_Y*10,ROBOT_MAX_Y*10); 284 | old_target_position_x = target_position_x; 285 | old_target_position_y = target_position_y; 286 | target_position_x = (float)target_x_mm*X_AXIS_STEPS_PER_UNIT/10.0; 287 | target_position_y = (float)target_y_mm*Y_AXIS_STEPS_PER_UNIT/10.0; 288 | // Speed adjust to draw straight lines 289 | diff_x = myAbs(target_position_x - old_target_position_x); 290 | diff_y = myAbs(target_position_y - old_target_position_y); 291 | if (diff_x > diff_y) // Wich axis will be slower? 292 | { 293 | com_speed_x = max_speed_x; 294 | com_speed_y = (float)max_speed_y*(float)diff_y/(float)diff_x; 295 | setSpeedS(com_speed_x,com_speed_y); 296 | } 297 | else 298 | { 299 | com_speed_x = (float)max_speed_x*(float)diff_x/(float)diff_y; 300 | com_speed_y = max_speed_y; 301 | setSpeedS(com_speed_x,com_speed_y); 302 | } 303 | } 304 | 305 | 306 | // Set speed in steps/sec 307 | void setSpeedS(int target_sx, int target_sy) 308 | { 309 | target_sx = constrain(target_sx,0,MAX_SPEED_X); 310 | target_sy = constrain(target_sy,0,MAX_SPEED_Y); 311 | target_speed_x = target_sx; 312 | target_speed_y = target_sy; 313 | } 314 | 315 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | Self Balance arduino robot. Control via Smartphone. Fully 3D printed project. 294 | Copyright (C) 2013 Jose Julio 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Air Hockey Robot (AHRobot) 2 | ========================== 3 | 4 | Detailed build intructions (Robot and Air Hockey table) [English]: https://docs.google.com/document/d/1SXIIW5MG1uT7O6P6tBWwzjiLe_gj4_Z2HHhHXPehwso/edit?usp=sharing 5 | 6 | Intrucciones de montaje de la mesa y el robot[Español]: https://docs.google.com/document/d/1hYupLZCxkPqST7Fa1OjUVXbfluQ60mGxMKnmPQMFq5Q/edit?usp=sharing 7 | 8 | 9 | Video: https://www.youtube.com/watch?v=CjzSeOg8oTs 10 | 11 | Air Hockey Robot build using standard 3D printer parts: electronics, motors, rods, belts,... 12 | 13 | Project description (English): http://cienciaycacharreo.blogspot.com.es/2014/02/new-project-air-hockey-robot-3d-printer.html 14 | 15 | Blog del proyecto (Spanish): http://cienciaycacharreo.blogspot.com.es/2014/02/nuevo-proyecto-air-hockey-robot-3d.html 16 | 17 | 18 | 19 | Jose Julio 2014 20 | 21 | 22 | -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/AHR.bat: -------------------------------------------------------------------------------- 1 | AHR.exe COM19 70 94 60 150 10 146 5 20 110 200 90 200 60 -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/AHR.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/AHR.exe -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/CHECK_HSV.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/CHECK_HSV.exe -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/opencv_core247.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/opencv_core247.dll -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/opencv_ffmpeg247.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/opencv_ffmpeg247.dll -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/opencv_highgui247.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/opencv_highgui247.dll -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/opencv_imgproc247.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/opencv_imgproc247.dll -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Binaries/opencv_video247.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JJulio/AHRobot/f5dbc4ef7b5ba183acb715174aa262e831e4c424/VisionSystem/PS3Camera/Binaries/opencv_video247.dll -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/CHECK_HSV.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | int lowerH=70; 7 | int lowerS=80; 8 | int lowerV=10; 9 | 10 | int upperH=94; 11 | int upperS=150; 12 | int upperV=125; 13 | 14 | //This function threshold the HSV image and create a binary image 15 | IplImage* GetThresholdedImage(IplImage* imgHSV){ 16 | 17 | IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1); 18 | cvInRangeS(imgHSV, cvScalar(lowerH,lowerS,lowerV), cvScalar(upperH,upperS,upperV), imgThresh); 19 | 20 | return imgThresh; 21 | 22 | } 23 | 24 | //This function create two windows and 6 trackbars for the "Ball" window 25 | void setwindowSettings(){ 26 | cvNamedWindow("Video"); 27 | cvNamedWindow("Ball"); 28 | cvNamedWindow("Controls"); 29 | 30 | cvCreateTrackbar("LowerH", "Controls", &lowerH, 180, NULL); 31 | cvCreateTrackbar("UpperH", "Controls", &upperH, 180, NULL); 32 | 33 | cvCreateTrackbar("LowerS", "Controls", &lowerS, 256, NULL); 34 | cvCreateTrackbar("UpperS", "Controls", &upperS, 256, NULL); 35 | 36 | cvCreateTrackbar("LowerV", "Controls", &lowerV, 256, NULL); 37 | cvCreateTrackbar("UpperV", "Controls", &upperV, 256, NULL); 38 | } 39 | 40 | int main(){ 41 | CvCapture* capture =0; 42 | 43 | capture = cvCaptureFromCAM(0); 44 | 45 | if(!capture){ 46 | printf("Capture failure\n"); 47 | return -1; 48 | } 49 | 50 | cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,320); 51 | cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,240); 52 | cvSetCaptureProperty(capture,CV_CAP_PROP_FPS,50); 53 | 54 | IplImage* frame=0; 55 | 56 | setwindowSettings(); 57 | 58 | //iterate through each frames of the video 59 | while(true){ 60 | 61 | frame = cvQueryFrame(capture); 62 | if(!frame) break; 63 | frame=cvCloneImage(frame); 64 | 65 | // optional load an image 66 | //frame = cvLoadImage("imagen3.png",CV_LOAD_IMAGE_COLOR ); 67 | 68 | cvSmooth(frame, frame, CV_GAUSSIAN,3,3); 69 | 70 | IplImage* imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3); 71 | cvCvtColor(frame, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV 72 | 73 | IplImage* imgThresh = GetThresholdedImage(imgHSV); 74 | 75 | cvShowImage("Ball", imgThresh); 76 | cvShowImage("Video", frame); 77 | 78 | //Clean up used images 79 | cvReleaseImage(&imgHSV); 80 | cvReleaseImage(&imgThresh); 81 | cvReleaseImage(&frame); 82 | 83 | //Wait 80mS 84 | int c = cvWaitKey(80); 85 | //If 'ESC' is pressed, break the loop 86 | if((char)c==27 ) break; 87 | 88 | } 89 | 90 | cvDestroyAllWindows(); 91 | cvReleaseCapture(&capture); 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /VisionSystem/PS3Camera/Source/AHR.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 11.00 3 | # Visual C++ Express 2010 4 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AHR", "AHR\AHR.vcxproj", "{794A26F5-2BDC-4CB0-A46E-5C08FF845082}" 5 | EndProject 6 | Global 7 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 8 | Debug|Win32 = Debug|Win32 9 | Release|Win32 = Release|Win32 10 | EndGlobalSection 11 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 12 | {794A26F5-2BDC-4CB0-A46E-5C08FF845082}.Debug|Win32.ActiveCfg = Debug|Win32 13 | {794A26F5-2BDC-4CB0-A46E-5C08FF845082}.Debug|Win32.Build.0 = Debug|Win32 14 | {794A26F5-2BDC-4CB0-A46E-5C08FF845082}.Release|Win32.ActiveCfg = Release|Win32 15 | {794A26F5-2BDC-4CB0-A46E-5C08FF845082}.Release|Win32.Build.0 = Release|Win32 16 | EndGlobalSection 17 | GlobalSection(SolutionProperties) = preSolution 18 | HideSolutionNode = FALSE 19 | EndGlobalSection 20 | EndGlobal 21 | -------------------------------------------------------------------------------- /VisionSystem/readme.txt: -------------------------------------------------------------------------------- 1 | AHRobot vision system: 2 | 3 | This vision system is designed for PS3 Eye camera and use OpenCV libraries 4 | 5 | If you want to compile from sources you need to install and configure OpenCV libraries --------------------------------------------------------------------------------