Hardware components | ||||||
| × | 1 | ||||
Hand tools and fabrication machines | ||||||
| ||||||
3d scanner using 3d printed parts, usb camera, beagle bone green or black, 2 nema 17 stepper motors and 12 mm green laser
Scans area using triangulation method. Design has some short comings, but is a good proof of concept.
https://www.youtube.com/watch?v=tok-qGXu-ZEPoint cloud of my office
Model
3d Scanner version 1.py
Python1st version of 3d scanner python script. less camera resolution faster scans but less accurate.
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
# by chris martin 6/25/2020
# Uses bits and pieces from the internet
# Python Script is for Python 2.7.16
# test on beaglebone bla
from Stepper_Motor_Control import motorcontrol #motor class
from PIL import Image # not really needed, but does help finding laser dot when setting up pixel locations
import select # Not sure what features this library all has
import time # maybe better to use Gevent. eventually
import v4l2capture # for usb camera
import subprocess #allows command line to be used in the program
import math #math and trig functions
# usb camera code http://www.elpcctv.com/mi5100-5mp-usb-camera-module-usb20-aptina-125inch-color-cmos-sensor-100degree-lens-p-221.html
video = v4l2capture.Video_device("/dev/video0") # access usb camera # Open the video device.
subprocess.check_call("v4l2-ctl -c exposure_absolute=150",shell=True) # dim exposure to reduce false positives defualt for camera is 500
subprocess.check_call("v4l2-ctl -c exposure_auto=1",shell=True) # dim exposure to reduce false positives
# Suggest an image size to the device. The device may choose and
# return another size if it doesn't support the suggested one.
Pixle_row_number=542 #grabbed from image need to make calibration routine.
H_resolution = 1600 #camera horizontal resolution
V_resolution = 1200 #camera vertical resolution
H_max_resolution = 2592 # elp 5 megapixle camera
V_max_resolution = 1944 # elp 5 megapixle camera
# 2592x1944 @ 15fps MJPEG Fails
# 2048x1536 @ 15fps MJPEG Works slower center is 690
# 1600x1200 @ 15fps MJPEG Works
# 1280x1024 @ 15fps MJPEG
# 1920x1080 @ 30fps MJPEG
# 1280x720 @ 30fps MJPEG
# 800x600 @ 30fps MJPEG center is 265
# 640x480 @ 30fps MJPEG
size_base_pixel = 0.0022 #mm from specs
h_sensor = H_max_resolution*size_base_pixel #sensor width
w_sensor = V_max_resolution*size_base_pixel #sensor height
f_camera = 3 #mm focal length
angle_tilt_degrees=50.90 # 3d model is 50 degrees use this to tune distance measurement
angle_tilt_rad=math.radians(angle_tilt_degrees) # convert tilt angle to radians for trig functions
Spacing=320.0 #mm between laser and camera center to pivot axis intersection longer than camera focus
centerdistancemeasured= 208.3 # measure this value using distance calibration program 208.5 for 2048x1536 210 for 800x600 209-210 for 1600x1200
size_x, size_y = video.set_format( H_resolution,V_resolution,0) #get actual camera set up resolution
H_pixle_width=h_sensor/size_x #lower resolutions group pixles one pixle
V_pixle_width=w_sensor/size_y #lower resolutions group pixles one pixle
#camera data set up
print "size:", size_x, size_y # display resolution
# Create a buffer to store image data in. This must be done before
# calling 'start' if v4l2capture is compiled with libv4l2. Otherwise
# raises IOError.
video.create_buffers(1)
# Start the device. This lights the LED if it's a camera that has one.
video.start()
# Send the buffer to the device.
video.queue_all_buffers()
# Wait a little. Some cameras take a few seconds to get bright enough.
time.sleep(.2)
print "camera set up"
# End Camera and scanner setup
# create stepper motor control
mc=motorcontrol() # mc stepper motor
freq=500 #pwm frequency
centeringrevs=.002
Nstepsperrev=200.0 # physical steps per rev 1.8 deg stepper motor tested at 200
m1=1
m2=1
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
if m1==0 and m2 == 0:
multipier=1
if m1==1 and m2 == 0:
multipier=4
if m1==0 and m2 == 1:
multipier=16
if m1==1 and m2 == 1:
multiplier=32
mc.armspin(0,1,freq,m1,m2,100) # lock arm duty=100
mc.zaxis(0,1,freq,m1,m2,100) #lock zaxis duty=100
# Find center of scanner base does not work the best, but is somewhat repeatable, would be better to have encoder
# program looks for laser point, counts how many pixles wide it is and calcs the average (center)
# the distance is measured many time and compared with actual center distance to pivot to determine center of scanner
# after center is found the arm, is rotated to horizontal
centerfound = 0 #intialize center finding loop
retry=0 # each measurement is taken multiple times
maxretries=5 # each measurement is taken multiple times
while not centerfound: #Loop to find center of base
mc.armspin(0,1,freq,m1,m2,100) # set arm pwm to 100 duty cycle locking it in place
select.select((video,), (), (), 1) #wait for camera to fill buffer
image_data = video.read_and_queue() #move camera buffer to program buffer
done = False # initialize inner loop (look for laser points)
i=size_x # initialize iterator
firstpoint=0 #intialize variable
numpointsfound = 0 #initialize variable
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
centerpoint =(firstpoint+firstpoint+numpointsfound)/2 #average location of green pixles found
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera) # angle between line from pixle average to focus and line from focus to center of sensor
distance=math.tan(angle_tilt_rad+sigma)*Spacing # calulate distance from pivot plane to laser point
distancetocenter=math.sqrt(distance*distance+((Spacing/2)*(Spacing/2))) # calulate distance from pivot to laser point
print "distance to center :",distancetocenter
if distancetocenter >=(centerdistancemeasured) and distancetocenter<(centerdistancemeasured+1): #look for center
print "centered"
centerfound=1
else:
if retry >=maxretries: # rotate arm one step if not found
mc.armspin(1,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(centeringrevs)*float(((Nstepsperrev*multiplier)/freq)) #time for stepper to run at frequency to rotate desired revolutions
time.sleep(runtime) #run stepper for runtime
retry=0
else:
retry=retry+1
#level arm after centered
mc.armspin(0,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(.3711)*float(((Nstepsperrev*multiplier)/freq)) # rotate from centered to horizontal/ revolutions form model
print runtime
time.sleep(runtime)
mc.armspin(0,1,freq,m1,m2,100)
time.sleep(3)
#scanning routine
pi=math.pi #pi constant
zaxisrevs=0 # intialize zaxis revolution counter
armrevstep=.005 # number of revs between each measurement
zaxisrevstep=.005 # number revs to turn zaxis each step
freq=150
select.select((video,), (), (), 1)
image_data1 = video.read_and_queue() #read image one time befor starting otherwise image is one step behind
# scan loop
loopend=0 #initialize looping variable
f = open('points2.xyz', 'a') #open point file for appending scan points xyz ascii data
while not loopend:
mc.zaxis(0,1,freq,m1,m2,100) # lock z axis
exit=0
armrevs=0 # intialize arm revolution counter
while not exit:
mc.armspin(0,1,freq,m1,m2,100) #hold arm
select.select((video,), (), (), 1)
image_data = video.read_and_queue()
done = False # initialize loop variable
i=size_x # initialize iterator
firstpoint=0
numpointsfound = 0
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
if numpointsfound >1: # only calculate point coords if point is found
centerpoint=(firstpoint+firstpoint+numpointsfound)/2
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera)
distance=math.tan(angle_tilt_rad+sigma)*Spacing
cosalpha=math.cos(armrevs*2*pi) # cosine
sinalpha=math.sin(armrevs*2*pi) # sine
costheta=math.cos(zaxisrevs*2*pi) # cosine zaxis rotation
sintheta=math.sin(zaxisrevs*2*pi) # cosine zaxis rotation
# pre rotation point on xz plane
# distance to pivot/2, distance
# rotate about y axis first y stays constant
xprime=((Spacing/2)*cosalpha-distance*sinalpha)
zprime=((Spacing/2)*sinalpha+distance*cosalpha)
yprime=0
#rotate about z axis second z stays constant
x=-1*(xprime*costheta)
y=(xprime*sintheta)
z=(zprime)
print>>f, str(x),str(y),str(z),'50','50','50', '\n'
armrevs=armrevs+armrevstep #count up arm revolutions
mc.armspin(0,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(armrevstep)*float(((Nstepsperrev*multiplier)/freq))
time.sleep(runtime-0.0012) #rotation time offest by program scan time until stop command is given
if armrevs>=.995:
exit=1 # end arm rotation after once around
mc.armspin(0,1,freq,m1,m2,100) # lock arm
zaxisrevs=zaxisrevs+zaxisrevstep #count up zaxis revolutions
mc.zaxis(0,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(zaxisrevstep)*float(((Nstepsperrev*multiplier)/freq))
time.sleep(runtime) #-0.0012)
print zaxisrevs
if zaxisrevs>=.5:
loopend=1
f.close() #close xyz file
mc.cutpower()
video.close()
pause=input()
3d Scanner version 2.py
Python2nd version of 3d scanner python script. less camera resolution faster scans but less accurate. fastest version of scanner, runs arm spin separate from data capture.
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
# by chris martin 6/25/2020
# Uses bits and pieces from the internet
# Python Script is for Python 2.7.16
# test on beaglebone bla
from Stepper_Motor_Control import motorcontrol #motor class
from PIL import Image # not really needed, but does help finding laser dot when setting up pixel locations
import select # Not sure what features this library all has
import time # maybe better to use Gevent. eventually
import v4l2capture # for usb camera
import subprocess #allows command line to be used in the program
import math #math and trig functions
# usb camera code http://www.elpcctv.com/mi5100-5mp-usb-camera-module-usb20-aptina-125inch-color-cmos-sensor-100degree-lens-p-221.html
video = v4l2capture.Video_device("/dev/video0") # access usb camera # Open the video device.
subprocess.check_call("v4l2-ctl -c exposure_absolute=150",shell=True) # dim exposure to reduce false positives defualt for camera is 500
subprocess.check_call("v4l2-ctl -c exposure_auto=1",shell=True) # dim exposure to reduce false positives
# Suggest an image size to the device. The device may choose and
# return another size if it doesn't support the suggested one.
Pixle_row_number=542 #grabbed from image need to make calibration routine.
H_resolution = 1600 #camera horizontal resolution
V_resolution = 1200 #camera vertical resolution
H_max_resolution = 2592 # elp 5 megapixle camera
V_max_resolution = 1944 # elp 5 megapixle camera
# 2592x1944 @ 15fps MJPEG Fails
# 2048x1536 @ 15fps MJPEG Works slower center is 690
# 1600x1200 @ 15fps MJPEG Works
# 1280x1024 @ 15fps MJPEG
# 1920x1080 @ 30fps MJPEG
# 1280x720 @ 30fps MJPEG
# 800x600 @ 30fps MJPEG center is 265
# 640x480 @ 30fps MJPEG
size_base_pixel = 0.0022 #mm from specs
h_sensor = H_max_resolution*size_base_pixel #sensor width
w_sensor = V_max_resolution*size_base_pixel #sensor height
f_camera = 3 #mm focal length
angle_tilt_degrees=50.90 # 3d model is 50 degrees use this to tune distance measurement
angle_tilt_rad=math.radians(angle_tilt_degrees) # convert tilt angle to radians for trig functions
Spacing=320.0 #mm between laser and camera center to pivot axis intersection longer than camera focus
centerdistancemeasured= 208.3 # measure this value using distance calibration program 208.5 for 2048x1536 210 for 800x600 209-210 for 1600x1200
size_x, size_y = video.set_format( H_resolution,V_resolution,0) #get actual camera set up resolution
H_pixle_width=h_sensor/size_x #lower resolutions group pixles one pixle
V_pixle_width=w_sensor/size_y #lower resolutions group pixles one pixle
#camera data set up
print "size:", size_x, size_y # display resolution
# Create a buffer to store image data in. This must be done before
# calling 'start' if v4l2capture is compiled with libv4l2. Otherwise
# raises IOError.
video.create_buffers(1)
# Start the device. This lights the LED if it's a camera that has one.
video.start()
# Send the buffer to the device.
video.queue_all_buffers()
# Wait a little. Some cameras take a few seconds to get bright enough.
time.sleep(.2)
print "camera set up"
# End Camera and scanner setup
# create stepper motor control
mc=motorcontrol() # mc stepper motor
freq=500 #pwm frequency
centeringrevs=.005
Nstepsperrev=200.0 # physical steps per rev 1.8 deg stepper motor tested at 200
m1=1
m2=1
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
if m1==0 and m2 == 0:
multipier=1
if m1==1 and m2 == 0:
multipier=4
if m1==0 and m2 == 1:
multipier=16
if m1==1 and m2 == 1:
multiplier=32
mc.armspin(0,1,freq,m1,m2,100) # lock arm duty=100
mc.zaxis(0,1,freq,m1,m2,100) #lock zaxis duty=100
# Find center of scanner base does not work the best, but is somewhat repeatable, would be better to have encoder
# program looks for laser point, counts how many pixles wide it is and calcs the average (center)
# the distance is measured many time and compared with actual center distance to pivot to determine center of scanner
# after center is found the arm, is rotated to horizontal
centerfound = 0 #intialize center finding loop
retry=0 # each measurement is taken multiple times
maxretries=5 # each measurement is taken multiple times
while not centerfound: #Loop to find center of base
mc.armspin(0,1,freq,m1,m2,100) # set arm pwm to 100 duty cycle locking it in place
select.select((video,), (), (), 1) #wait for camera to fill buffer
image_data = video.read_and_queue() #move camera buffer to program buffer
done = False # initialize inner loop (look for laser points)
i=size_x # initialize iterator
firstpoint=0 #intialize variable
numpointsfound = 0 #initialize variable
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
centerpoint =(firstpoint+firstpoint+numpointsfound)/2 #average location of green pixles found
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera) # angle between line from pixle average to focus and line from focus to center of sensor
distance=math.tan(angle_tilt_rad+sigma)*Spacing # calulate distance from pivot plane to laser point
distancetocenter=math.sqrt(distance*distance+((Spacing/2)*(Spacing/2))) # calulate distance from pivot to laser point
print "distance to center :",distancetocenter
if distancetocenter >=(centerdistancemeasured) and distancetocenter<(centerdistancemeasured+1): #look for center
print "centered"
centerfound=1
else:
if retry >=maxretries: # rotate arm one step if not found
mc.armspin(1,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(centeringrevs)*float(((Nstepsperrev*multiplier)/freq)) #time for stepper to run at frequency to rotate desired revolutions
time.sleep(runtime) #run stepper for runtime
retry=0
else:
retry=retry+1
#level arm after centered
mc.armspin(0,1,freq,m1,m2,50) #armspin(self,direction,active,frequency,m1,m2):
runtime=float(.3711)*float(((Nstepsperrev*multiplier)/freq)) # rotate from centered to horizontal/ revolutions form model
print runtime
time.sleep(runtime)
mc.armspin(0,1,freq,m1,m2,100)
time.sleep(3)
#scanning routine
pi=math.pi #pi constant
zaxisrevs=0 # intialize zaxis revolution counter
zaxisrevstep=.001 # number revs to turn zaxis each step
freq=50
zfreq=100
select.select((video,), (), (), 1)
image_data1 = video.read_and_queue() #read image one time befor starting otherwise image is one step behind
starttime=(time.time()+.052)
mc.armspin(0,1,freq,m1,m2,50)
# scan loop
loopend=0 #initialize looping variable
f = open('pointsversion2.xyz', 'a') #open point file for appending scan points xyz ascii data
while not loopend:
mc.zaxis(0,1,freq,m1,m2,100) # lock z axis
exit=0
while not exit:
timeimagecapture=time.time()
select.select((video,), (), (), 1)
image_data = video.read_and_queue()
done = False # initialize loop variable
i=size_x # initialize iterator
firstpoint=0
numpointsfound = 0
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
if numpointsfound >1: # only calculate point coords if point is found
runtime=timeimagecapture-starttime
armrevs= runtime/(float(((Nstepsperrev*multiplier)/freq)))
centerpoint=(firstpoint+firstpoint+numpointsfound)/2
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera)
distance=math.tan(angle_tilt_rad+sigma)*Spacing
cosalpha=math.cos(armrevs*2*pi) # cosine
sinalpha=math.sin(armrevs*2*pi) # sine
costheta=math.cos(zaxisrevs*2*pi) # cosine zaxis rotation
sintheta=math.sin(zaxisrevs*2*pi) # cosine zaxis rotation
# pre rotation point on xz plane
# distance to pivot/2, distance
# rotate about y axis first y stays constant
xprime=((Spacing/2)*cosalpha-distance*sinalpha)
zprime=((Spacing/2)*sinalpha+distance*cosalpha)
yprime=0
#rotate about z axis second z stays constant
x=-1*(xprime*costheta)
y=(xprime*sintheta)
z=(zprime)
print>>f, str(x),str(y),str(z),'50','50','50', '\n'
print cosalpha
if cosalpha>=0.9999:
exit=1 # end arm rotation after once around
zaxisrevs=zaxisrevs+zaxisrevstep #count up zaxis revolutions
mc.zaxis(0,1,zfreq,m1,m2,50)
runtime=float(zaxisrevstep)*float(((Nstepsperrev*multiplier)/zfreq))
time.sleep(runtime) #-0.0012)
print zaxisrevs
if zaxisrevs>=.5:
loopend=1
f.close() #close xyz file
mc.cutpower()
video.close()
pause=input()
3d Scanner version 3.py
Python3rd version of 3d scanner python script. less camera resolution faster scans but less accurate. runs steppers a set number of pulses versus using timers.
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
need to calibrate tilt angle, spacing, etc to get distances correct. need to change distance to center to get centering loop to work
# by chris martin 6/25/2020
# Uses bits and pieces from the internet
# Python Script is for Python 2.7.16
# test on beaglebone bla
from Stepper_Motor_Control_2 import motorcontrol #motor class
from PIL import Image # not really needed, but does help finding laser dot when setting up pixel locations
import select # Not sure what features this library all has
import time # maybe better to use Gevent. eventually
import v4l2capture # for usb camera
import subprocess #allows command line to be used in the program
import math #math and trig functions
# usb camera code http://www.elpcctv.com/mi5100-5mp-usb-camera-module-usb20-aptina-125inch-color-cmos-sensor-100degree-lens-p-221.html
video = v4l2capture.Video_device("/dev/video0") # access usb camera # Open the video device.
subprocess.check_call("v4l2-ctl -c exposure_absolute=150",shell=True) # dim exposure to reduce false positives defualt for camera is 500
subprocess.check_call("v4l2-ctl -c exposure_auto=1",shell=True) # dim exposure to reduce false positives
# Suggest an image size to the device. The device may choose and
# return another size if it doesn't support the suggested one.
Pixle_row_number=542 #grabbed from image need to make calibration routine.
H_resolution = 1600 #camera horizontal resolution
V_resolution = 1200 #camera vertical resolution
H_max_resolution = 2592 # elp 5 megapixle camera
V_max_resolution = 1944 # elp 5 megapixle camera
# 2592x1944 @ 15fps MJPEG Fails
# 2048x1536 @ 15fps MJPEG Works slower center is 690
# 1600x1200 @ 15fps MJPEG Works
# 1280x1024 @ 15fps MJPEG
# 1920x1080 @ 30fps MJPEG
# 1280x720 @ 30fps MJPEG
# 800x600 @ 30fps MJPEG center is 265
# 640x480 @ 30fps MJPEG
size_base_pixel = 0.0022 #mm from specs
h_sensor = H_max_resolution*size_base_pixel #sensor width
w_sensor = V_max_resolution*size_base_pixel #sensor height
f_camera = 3 #mm focal length
angle_tilt_degrees=50.90 # 3d model is 50 degrees use this to tune distance measurement
angle_tilt_rad=math.radians(angle_tilt_degrees) # convert tilt angle to radians for trig functions
Spacing=320.0 #mm between laser and camera center to pivot axis intersection longer than camera focus
centerdistancemeasured= 208.3 # measure this value using distance calibration program 208.5 for 2048x1536 210 for 800x600 209-210 for 1600x1200
size_x, size_y = video.set_format( H_resolution,V_resolution,0) #get actual camera set up resolution
H_pixle_width=h_sensor/size_x #lower resolutions group pixles one pixle
V_pixle_width=w_sensor/size_y #lower resolutions group pixles one pixle
#camera data set up
print "size:", size_x, size_y # display resolution
# Create a buffer to store image data in. This must be done before
# calling 'start' if v4l2capture is compiled with libv4l2. Otherwise
# raises IOError.
video.create_buffers(1)
# Start the device. This lights the LED if it's a camera that has one.
video.start()
# Send the buffer to the device.
video.queue_all_buffers()
# Wait a little. Some cameras take a few seconds to get bright enough.
time.sleep(.2)
print "camera set up"
# End Camera and scanner setup
# create stepper motor control
mc=motorcontrol() # mc stepper motor
freq=500 #pwm frequency
centeringrevs=.005
Nstepsperrev=200.0 # physical steps per rev 1.8 deg stepper motor tested at 200
centeringspeed=400
m1=1
m2=1
mc.armspin2(0,.001,centeringspeed,m1,m2) # lock arm duty=100
mc.zaxis2(0,.001,centeringspeed,m1,m2) #lock zaxis duty=100
# Find center of scanner base does not work the best, but is somewhat repeatable, would be better to have encoder
# program looks for laser point, counts how many pixles wide it is and calcs the average (center)
# the distance is measured many time and compared with actual center distance to pivot to determine center of scanner
# after center is found the arm, is rotated to horizontal
centerfound = 0 #intialize center finding loop
retry=0 # each measurement is taken multiple times
maxretries=5 # each measurement is taken multiple times
while not centerfound: #Loop to find center of base
select.select((video,), (), (), 1) #wait for camera to fill buffer
image_data = video.read_and_queue() #move camera buffer to program buffer
done = False # initialize inner loop (look for laser points)
i=size_x # initialize iterator
firstpoint=0 #intialize variable
numpointsfound = 0 #initialize variable
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
centerpoint =(firstpoint+firstpoint+numpointsfound)/2 #average location of green pixles found
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera) # angle between line from pixle average to focus and line from focus to center of sensor
distance=math.tan(angle_tilt_rad+sigma)*Spacing # calulate distance from pivot plane to laser point
distancetocenter=math.sqrt(distance*distance+((Spacing/2)*(Spacing/2))) # calulate distance from pivot to laser point
print "distance to center :",distancetocenter
if distancetocenter >=(centerdistancemeasured) and distancetocenter<(centerdistancemeasured+1): #look for center
print "centered"
centerfound=1
else:
if retry >=maxretries: # rotate arm one step if not found
mc.armspin2(1,centeringrevs,centeringspeed,m1,m2) #armspin(self,direction,active,frequency,m1,m2):
retry=0
else:
retry=retry+1
#level arm after centered
mc.armspin2(0,.3711,centeringspeed*2,m1,m2) #armspin(self,direction,active,frequency,m1,m2):
time.sleep(3)
#scanning routine
pi=math.pi #pi constant
zaxisrevs=0 # intialize zaxis revolution counter
scanningspeed=350
rotatezspeed=200
select.select((video,), (), (), 1)
image_data1 = video.read_and_queue() #read image one time befor starting otherwise image is one step behind
armrevstep=.005
zaxisrevstep=.0025 # number revs to turn zaxis each step
# scan loop
loopend=0 #initialize looping variable
f = open('points2.xyz', 'a') #open point file for appending scan points xyz ascii data
while not loopend:
exit=0
armrevs=0 # intialize arm revolution counter
while not exit:
select.select((video,), (), (), 1)
image_data = video.read_and_queue()
done = False # initialize loop variable
i=size_x # initialize iterator
firstpoint=0
numpointsfound = 0
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution #image buffer is series of 3 color tupples offest is hwo far into the buffer the start of the center row or column is.
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1])) #read green
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3])) #read red
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2])) #read blue
if ((rawpointcolorg>245) and (rawpointcolorr<245)): # look for green laser, filter out some bright light, by using red value
if firstpoint == 0: #find first pixle that is green
firstpoint =i
numpointsfound += 1 # count green pixels found
if i==0 or (i < (firstpoint-30)): # if no green pixle found or 30 pixles past the first one found are checked then stop looking
done=True
if numpointsfound >1: # only calculate point coords if point is found
centerpoint=(firstpoint+firstpoint+numpointsfound)/2
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera)
distance=math.tan(angle_tilt_rad+sigma)*Spacing
cosalpha=math.cos(armrevs*2*pi) # cosine
sinalpha=math.sin(armrevs*2*pi) # sine
costheta=math.cos(zaxisrevs*2*pi) # cosine zaxis rotation
sintheta=math.sin(zaxisrevs*2*pi) # cosine zaxis rotation
# pre rotation point on xz plane
# distance to pivot/2, distance
# rotate about y axis first y stays constant
xprime=((Spacing/2)*cosalpha-distance*sinalpha)
zprime=((Spacing/2)*sinalpha+distance*cosalpha)
yprime=0
#rotate about z axis second z stays constant
x=-1*(xprime*costheta)
y=(xprime*sintheta)
z=(zprime)
print>>f, str(x),str(y),str(z),'50','50','50', '\n'
armrevs=armrevs+armrevstep #count up arm revolutions
mc.armspin2(0,armrevstep,scanningspeed,m1,m2) #armspin(self,direction,active,frequency,m1,m2):
if armrevs>=1:
exit=1 # end arm rotation after once around
print armrevs
zaxisrevs=zaxisrevs+zaxisrevstep #count up zaxis revolutions
mc.zaxis2(0,zaxisrevstep,rotatezspeed,m1,m2) #armspin(self,direction,active,frequency,m1,m2):
print zaxisrevs
if zaxisrevs>=.5:
loopend=1
f.close() #close xyz file
mc.cutpower()
video.close()
pause=input()
calibration help tool.py
Pythonhelps to set up scan script by finding centering distance and other tasks. calibrate tilt angle with tape measure.
#!/usr/bin/python
# https://github.com/gebart/python-v4l2capture/blob/master/capture_picture_delayed.py
# python-v4l2capture
#
# This file is an example on how to capture a picture with
# python-v4l2capture. It waits between starting the video device and
# capturing the picture, to get a good picture from cameras that
# require a delay to get enough brightness. It does not work with some
# devices that require starting to capture pictures immediatly when
# the device is started.
#
# 2009, 2010 Fredrik Portstrom
#
# I, the copyright holder of this file, hereby release it into the
# public domain. This applies worldwide. In case this is not legally
# possible: I grant anyone the right to use this work for any
# purpose, without any conditions, unless such conditions are
# required by law.
from PIL import Image
import select
import time
import v4l2capture
import subprocess
import math
# Open the video device.
# too list cameras v4l2-ctl --list-devices
print time.clock()
video = v4l2capture.Video_device("/dev/video0")
subprocess.check_call("v4l2-ctl -c exposure_absolute=150",shell=True)
subprocess.check_call("v4l2-ctl -c exposure_auto=1",shell=True)
# subprocess.check_call("v4l2-ctl -D -l",shell=True)
# Suggest an image size to the device. The device may choose and
# return another size if it doesn't support the suggested one.
Pixle_row_number=542 #grabbed from image need to make calibration routine.
H_resolution = 1600
V_resolution = 1200
H_max_resolution = 2592 # elp 5 megapixle camera
V_max_resolution = 1944 # elp 5 megapixle camera
size_base_pixel = 0.0022 #mm
h_sensor = H_max_resolution*size_base_pixel #sensor width
w_sensor = V_max_resolution*size_base_pixel #sensor height
f_camera = 3 #mm focal length
angle_tilt_degrees=50.675
offset_to_pivot=00 #mm
angle_tilt_rad=math.radians(angle_tilt_degrees)
print angle_tilt_rad, " tilt angle"
Spacing=320.0 #mm between laser and camera focus
size_x, size_y = video.set_format( H_resolution,V_resolution,0)
H_pixle_width=h_sensor/size_x #lower resolutions group pixles one pixle
V_pixle_width=w_sensor/size_y #lower resolutions group pixles one pixle
print "size:", size_x, size_y
print "1"
# Create a buffer to store image data in. This must be done before
# calling 'start' if v4l2capture is compiled with libv4l2. Otherwise
# raises IOError.
video.create_buffers(1)
print "2"
# Start the device. This lights the LED if it's a camera that has one.
video.start()
# Send the buffer to the device.
print "3"
video.queue_all_buffers()
# Wait a little. Some cameras take a few seconds to get bright enough.
# time.sleep(2)
print "4"
# Wait for the device to fill the buffer.
snapshot=0
averager=0
print time.clock()
while True:
select.select((video,), (), (), 5)
image_data = video.read_and_queue()
snapshot=snapshot+1
done = False
i=size_x # initialize iterator
firstpoint=0
numpointsfound = 0
while not done:
i=i-1
# read raw r,g,b
#0,0 1,0.......799,0
#.
#
#599,0 599,1.....599,799
Offset=Pixle_row_number*H_resolution
rawpointcolorg=ord(bytearray(image_data[(i+Offset)*3+1]))
rawpointcolorr=ord(bytearray(image_data[(i+Offset)*3]))
rawpointcolorb=ord(bytearray(image_data[(i+Offset)*3+2]))
if ((rawpointcolorg>240) and (rawpointcolorr<245)):
print i,Pixle_row_number, ":", rawpointcolorr,rawpointcolorg, rawpointcolorb
if firstpoint == 0:
firstpoint =i
numpointsfound += 1
if i==0 or (i < (firstpoint-20)):
done=True
centerpoint =(firstpoint+firstpoint+numpointsfound)/2
sigma=math.atan((H_pixle_width*((centerpoint)-(size_x/2)))/f_camera)
distance=math.tan(angle_tilt_rad+sigma)*Spacing+offset_to_pivot
distancetocenter=math.sqrt(distance*distance+((Spacing/2)*(Spacing/2))) # calulate distance from pivot to laser point
print "angle:", sigma,"distance :",distance/25.4, distancetocenter
averager=averager+distance
if snapshot >=10:
print (averager/(snapshot*25.4))
print snapshot
print time.clock()
break
image = Image.frombuffer("RGB", (size_x, size_y), image_data,'raw',"RGB",0,1)
video.close()
f = open('my_file', 'w+b')
f.write(image_data)
f.close()
image.save("image.jpg")
print "number of points found ", numpointsfound
print "Saved image.jpg (Size: " + str(size_x) + " x " + str(size_y) + ")"
print "done"
#subprocess.check_call("v4l2-ctl -D -l",shell=True)
#motor control
import Adafruit_BBIO.GPIO as GPIO
import Adafruit_BBIO.PWM as PWM
import time
import subprocess
# NOTES
# variable to set frequency and pulse width for testing
#freq = 500 #Hz
#duty = 50 #%
#PWM.start("P8_34",duty,freq,0) #PWM1A pin P8-36
#PWM.start("P9_31",duty,freq,0) #PWM1A pin P8-36
# need to set pin to pwm # config-pin -a p9.31 pwm
#PWM.start("P8_45",duty,freq,0) #PWM2A pin P8-45
# need to set pin to pwm # config-pin -a p8.45 pwm
class motorcontrol(object):
#globals for testing
# intitialize
def __init__(self):
# line runs bash command to config pin, if the cape manager is not loaded it will be triggered to load doesnt matter what pin is set
#subprocess.check_call("config-pin -a p9.14 pwm",shell=True)
# Stepper Motor Control 1 & 2 (Camera arm spin and z axis)
# set up pins to run stepper motor pulse & direction
#armspin
GPIO.setup("P9_11", GPIO.OUT) # pinarmspinreset
GPIO.setup("P9_13", GPIO.OUT) # pinarmspinM1
GPIO.setup("P9_15", GPIO.OUT) # pinarmspinM2
GPIO.setup("P9_17", GPIO.OUT) # pinarmspindir
#zxis
GPIO.setup("P9_23", GPIO.OUT) # pinzaxisreset
GPIO.setup("P9_25", GPIO.OUT) # pinzaxisM1
GPIO.setup("P9_27", GPIO.OUT) # pinzaxisM2
GPIO.setup("P9_24", GPIO.OUT) # pinzaxisdir
print "initilized"
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
# destructor
def __del__(self):
#armspin
PWM.stop("P9_21") #pin P9-21 PWM
GPIO.output("P9_11", 0) # pinarmspinreset
GPIO.output("P9_13", 0) # pinarmspinM1
GPIO.output("P9_15", 0) # pinarmspinM2
GPIO.output("P9_17", 0) # pinarmspindir
#zxis
PWM.stop("P9_14") #pin P9-31 PWM
GPIO.output("P9_23", 0) # pinzaxisreset
GPIO.output("P9_25", 0) # pinzaxisM1
GPIO.output("P9_27", 0) # pinzaxisM2
GPIO.output("P9_24", 0) # pinzaxisdir
PWM.cleanup()
print 'end'
def cutpower(self):
#armspin
PWM.stop("P9_21") #pin P9-21 PWM
GPIO.output("P9_11", 0) # pinarmspinreset
GPIO.output("P9_13", 0) # pinarmspinM1
GPIO.output("P9_15", 0) # pinarmspinM2
GPIO.output("P9_17", 0) # pinarmspindir
#zxis
PWM.stop("P9_14") #pin P9-31 PWM
GPIO.output("P9_23", 0) # pinzaxisreset
GPIO.output("P9_25", 0) # pinzaxisM1
GPIO.output("P9_27", 0) # pinzaxisM2
GPIO.output("P9_24", 0) # pinzaxisdir
print 'end'
PWM.cleanup()
def armspin(self,direction,active,frequency,m1,m2, duty):
#freq = float(frequency) #Hz 20000 #Hz
PWM.start("P9_21",duty,frequency,0)
if active==1:
GPIO.output("P9_11", 1) # pinarmspinreset
GPIO.output("P9_13", m1) # pinarmspinM1
GPIO.output("P9_15", m2) # pinarmspinM2
GPIO.output("P9_17", direction) # pinarmspindir
if active==0:
PWM.stop("P9_21")
GPIO.output("P9_11", 1) # pinarmspinreset
GPIO.output("P9_13", 0) # pinarmspinM1
GPIO.output("P9_15", 0) # pinarmspinM2
GPIO.output("P9_17", 0) # pinarmspindir
PWM.cleanup()
def zaxis(self,direction,active,frequency,m1,m2,duty):
#freq = frequency #Hz 20000 #Hz
PWM.start('P9_14',duty,frequency,0)
if active==1:
GPIO.output("P9_23", 1) # pinzaxisreset
GPIO.output("P9_25", m1) # pinzaxisM1
GPIO.output("P9_27", m2) # pinzaxisM2
GPIO.output("P9_24", direction) # pinzaxisdir
if active==0:
PWM.stop("P9_14")
GPIO.output("P9_23", 1) # pinzaxisreset
GPIO.output("P9_25", 0) # pinzaxisM1
GPIO.output("P9_27", 0) # pinzaxisM2
GPIO.output("P9_24", 0) # pinzaxisdir
PWM.cleanup()
#motor control
import Adafruit_BBIO.GPIO as GPIO
import time
import subprocess
# NOTES
class motorcontrol(object):
#globals for testing
# intitialize
def __init__(self):
# line runs bash command to config pin,
subprocess.check_call("config-pin p9.14 gpio",shell=True)
subprocess.check_call("config-pin p9.21 gpio",shell=True)
# Stepper Motor Control 1 & 2 (Camera arm spin and z axis)
# set up pins to run stepper motor pulse & direction
#armspin
GPIO.setup("P9_11", GPIO.OUT) # pinarmspinreset
GPIO.setup("P9_13", GPIO.OUT) # pinarmspinM1
GPIO.setup("P9_15", GPIO.OUT) # pinarmspinM2
GPIO.setup("P9_17", GPIO.OUT) # pinarmspindir
GPIO.setup("P9_21", GPIO.OUT) # need to configure the pin as gpio vs pwm
#zxis
GPIO.setup("P9_23", GPIO.OUT) # pinzaxisreset
GPIO.setup("P9_25", GPIO.OUT) # pinzaxisM1
GPIO.setup("P9_27", GPIO.OUT) # pinzaxisM2
GPIO.setup("P9_24", GPIO.OUT) # pinzaxisdir
GPIO.setup("P9_14", GPIO.OUT) # need to configure the pin as gpio vs pwm
print "initilized"
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
# destructor
def __del__(self):
#armspin
GPIO.output("P9_11", 0) # pinarmspinreset
GPIO.output("P9_13", 0) # pinarmspinM1
GPIO.output("P9_15", 0) # pinarmspinM2
GPIO.output("P9_17", 0) # pinarmspindir
GPIO.output("P9_21", 0) # pulses
#zxis
GPIO.output("P9_23", 0) # pinzaxisreset
GPIO.output("P9_25", 0) # pinzaxisM1
GPIO.output("P9_27", 0) # pinzaxisM2
GPIO.output("P9_24", 0) # pinzaxisdir
GPIO.output("P9_14", 0) # pulses
print 'end'
def cutpower(self):
#armspin
GPIO.output("P9_11", 0) # pinarmspinreset
GPIO.output("P9_13", 0) # pinarmspinM1
GPIO.output("P9_15", 0) # pinarmspinM2
GPIO.output("P9_17", 0) # pinarmspindir
GPIO.output("P9_21", 0) # pulses
#zxis
GPIO.output("P9_23", 0) # pinzaxisreset
GPIO.output("P9_25", 0) # pinzaxisM1
GPIO.output("P9_27", 0) # pinzaxisM2
GPIO.output("P9_24", 0) # pinzaxisdir
GPIO.output("P9_14", 0) # pulses
print 'end'
def zaxis2(self,direction,revs,speed,m1,m2):
Nstepsperrev=200.0 # physical steps per rev 1.8 deg stepper motor tested at 200
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
if m1==0 and m2 == 0:
multiplier=1
if m1==1 and m2 == 0:
multiplier=4
if m1==0 and m2 == 1:
multiplier=16
if m1==1 and m2 == 1:
multiplier=32
GPIO.output("P9_23", 1) # pinzaxisreset
GPIO.output("P9_25", m1) # pinzaxisM1
GPIO.output("P9_27", m2) # pinzaxisM2
GPIO.output("P9_24", direction) # pinzaxisdir
done=0
pulses=float(0)
sleeptime=1/float(speed)
while not done:
GPIO.output("P9_14",1) #need to reconfigure pin or move to different pin
time.sleep(sleeptime)
GPIO.output("P9_14",0)
time.sleep(sleeptime)
pulses=pulses+float(1/(Nstepsperrev*multiplier))
if pulses >= (revs):
done =1
def armspin2(self,direction,revs,speed,m1,m2):
Nstepsperrev=200.0 # physical steps per rev 1.8 deg stepper motor tested at 200
# M1=0, M2=0 full step
# M1=1, M2=0 1/4 step
# M1=0, M2=1 1/16 step
# M1=1, M2=1 1/32 step
if m1==0 and m2 == 0:
multiplier=1
if m1==1 and m2 == 0:
multiplier=4
if m1==0 and m2 == 1:
multiplier=16
if m1==1 and m2 == 1:
multiplier=32
GPIO.output("P9_11", 1) # pinarmspinreset
GPIO.output("P9_13", m1) # pinarmspinM1
GPIO.output("P9_15", m2) # pinarmspinM2
GPIO.output("P9_17", direction) # pinarmspindir
done=0
pulses=float(0)
sleeptime=1/float(speed)
while not done:
GPIO.output("P9_21",1) #need to reconfigure pin or move to different pin
time.sleep(sleeptime)
GPIO.output("P9_21",0)
time.sleep(sleeptime)
pulses=pulses+float(1/(Nstepsperrev*multiplier))
if pulses >= (revs):
done =1
import time # maybe better to use Gevent. eventually
from Stepper_Motor_Control_2 import motorcontrol #motor class
starttime=time.time()
print starttime
time.sleep(0)
endtime=time.time()
print endtime
print endtime-starttime
mc=motorcontrol()
m1=1
m2=1
print "set up"
done=0
while not done:
print "enter revs:",
revs=input()
print int(revs)
print "enter speed",
speed=input()
if revs <>0:
mc.armspin2(0,revs,speed,m1,m2) #armspin(self,direction,active,frequency,m1,m2):
else:
done=1
print "done"
Comments