Zoals een laser pen schrijver die een italiaan (Daniele Tartaglia) op youtube heeft gemaakt, alleen gebruikte hij de kop-loopwerkjes (ivm stappenmotoren).
youtu.be/SamSyZtsA5E
Uiteindelijk wel iets werkend gekregen, maar niet voldoende goed werkend, dus bedacht ik mij dat ik de lade systemen ook maar met stappenmotoren moest uitvoeren
Hierdoor wordt een groter bereik gehaald, nl. 12,5 x 12,5 cm.
(Ipv 4x4cm bij die van Daniele)
Hieronder de laatste python code:
Code: Select all
#!/usr/bin/python
#
# DG2021
#
# 2 motor driven 2d draw robot (plotter?) from CD load facility.
# Idea from Daniele Tartaglia https://www.youtube.com/watch?v=Q5ma1HDuotk
# Only i do not use the head motors, i use the load-drawer mechanics.
#
# Drawbed will be aprox. 12.5 cm x 12.5cm
#
# Both motors have start- & end switches, are NOT steppen motors, just DC.
# We do still need 1 function to enable pen, to write ... magnetic ?
#
# For the program:
# - ask which file ... or pic to convert ...
# - Do we 1st mark left up - right bottom first, or only left up,
# due to shifting of motors & locations?
# - Always a 'dot' test of both motors to set max length?
# aka how many steps we can devide.
# - Does it draw line by row, or real 2d?
# What works best?
#
# Do we make a steppen motor one? (yes ?)
#
# For program
Pversion = 22 # Program version
#
# 22
# ---
# slower arm movement, more steps...
#
# ... 21
# ------
# Less arm movements?
# Line bar pattern test...
#
# 17 + 18
# -------
# Servo now has other positions?
# Is it broken?
# we adjust the angle values ...
# To do:
# - rewrite when pen down on co-ordinates X,Y
# - adjust Pen-servo angles on used pen.
# - Fix the pen head (left-right, on servo buzzing)
#
# 16
# ---
# defined end pos for pen, no obstictions.
#
# 15
# ---
# First test made, line by no-line ...
# We have to exclude first 10 lines & dots, otherwise the pen is down outside grid
# and can obstruct other motors.
#
# 14
# ---
# We made a pen with servo control !
# Testing...
#
#
# 13
# ---
# Until now we have:
# - declared and running motors & switches
# - ask for input file
# - ask for number of copies
# - a running line secuence
# - check HW procedure (10x)
# - motor movement subroutines
# - added DutyC2 for motor 2 (it may go faster than M1 ?)
# - arm goes to correct position after (each) print / plot (if not already)
# - list of error codes
#
# This 13 one we try using cursor-keys to move motors.
#
# - make 4 move subroutines.
# - exact numbering of 'steps' using the end-switches.
# specially for M2 close position (it has a longer run)
#
#
# Error codes:
# -------------
# parameter 'error' changed only during checkHW process, so:
# - M1 should during check always be out-wards (extended) position
# - M2 should during check always be in-wards (home) position
#
# 99 - Start value
# 0 - Ok value
# 1 - M1 between sw1 and sw2 (no sw activated)
# 2 - M1 not on end but on start position
# 3 - M2 between sw1 and sw2 (no sw activated)
# 4 - M2 not on start but on end position
# 10 - Both switches are activated for M1, this is not normal!
# 11 - Both switches are activated for M2, this is not normal!
#
# Time tests:
# ------------
# 15 minutes run on settings:
# setpulse = 0.01 shortest time to power motors, to 'make' steps
# waittime = 0.05 used during test & now delay for motors
# DutyC = 50 actual dutycycle to motors (power)
#
# 11 minutes run on settings:
# setpulse = 0.01 shortest time to power motors, to 'make' steps (seconds)
# waittime = 0.05 used during test & now delay for motors (seconds)
# DutyC = 50 actual dutycycle motor 1 (power)
# DutyC = 75 actual dutycycle motor 2 (power)
#
# 16 minutes run on settings: (lower batteries)
# setpulse = 0.01 shortest time to power motors, to 'make' steps (seconds)
# waittime = 0.005 used during test & now delay for motors (seconds)
# DutyC = 50 actual dutycycle motor 1 (power)
# DutyC = 75 actual dutycycle motor 2 (power)
#
#
# To do:
# - (time) check on motors ?
#
#
# DG.
#
# Default init
#
import RPi.GPIO as GPIO
from time import sleep, strftime
# We used the BOARD pin layout
GPIO.setmode(GPIO.BOARD)
# Define RPi IO ports to L293D chip to control 2 motors pwm
# m1in1 = 18 # -> L293D pin 2
# m1in2 = 22 # -> L293D pin 7
# m1ena = 16 # -> L293D pin 1
m1in1 = 33 # -> L293D pin 2
m1in2 = 35 # -> L293D pin 7
m1ena = 37 # -> L293D pin 1
m2in1 = 19 # -> L293D pin 15
m2in2 = 21 # -> L293D pin 10
m2ena = 23 # -> L293D pin 9
# are all outputs
GPIO.setup(m1in1,GPIO.OUT)
GPIO.setup(m1in2,GPIO.OUT)
GPIO.setup(m1ena,GPIO.OUT)
GPIO.setup(m2in1,GPIO.OUT)
GPIO.setup(m2in2,GPIO.OUT)
GPIO.setup(m2ena,GPIO.OUT)
# the pwm enablement pins for both motors are:
pwm1=GPIO.PWM(m1ena, 100)
pwm2=GPIO.PWM(m2ena, 100)
# set the dutycycle to 0 (no move)
pwm1.start(0)
pwm2.start(0)
# Due to pwm the actual enable function is set by DutcyCycle adjustment
# to anything else then 0.
# Remark that a DC motor needs at lease 30% ?? to actually starts moving.
# Some DC motors only 'buzz' at low speeds.
#
# Set default rotation motor 1
GPIO.output(m1in1, False)
GPIO.output(m1in2, True)
# Motor 2
GPIO.output(m2in1, False)
GPIO.output(m2in2, True)
# We do have to set the 4 end-switches of both motors still on GPIO
m1sw1 = 7 # -> start pos motor 1 sw1
m1sw2 = 11 # -> end pos motor 1 sw2
m2sw1 = 12 # -> end pos motor 2 sw1
m2sw2 = 13 # -> start pos motor 2 sw2
# are all inputs, on which a motor should STOP
GPIO.setup(m1sw1,GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(m1sw2,GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(m2sw1,GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(m2sw2,GPIO.IN, pull_up_down=GPIO.PUD_UP)
# Servo parameters
OFFSE_DUTY = 0.51 # define pulse offset of servo
SERVO_MIN_DUTY = 2.5+OFFSE_DUTY # define pulse duty cycle for minimum angle of servo
SERVO_MAX_DUTY = 12.5+OFFSE_DUTY # define pulse duty cycle for maximum angle of servo
servoPin = 16
SERVO_delay1 = 1
GPIO.setup(servoPin, GPIO.OUT) # Set servoPin's mode is output
GPIO.output(servoPin, GPIO.LOW) # Set servoPin to low
p = GPIO.PWM(servoPin,50) # set frequency to 50Hz
p.start(0) # Duty Cycle = 0
#
# Used (global?) parameters
#
# For motors / HW
setpulse = 0.01 # shortest time to power motors, to 'make' steps (seconds)
waittime = 0.005 # not used now - delay for motor 2 steps (seconds)
DutyC = 50 # actual dutycycle motor 1 (power)
DutyC2 = 50 # actual dutycycle motor 2 (power)
setState = False # used for motor 2 change rotation
retryC = 10 # Retry counter for HW test
# For Servo
servoStartAngle = 85 # Start position of servo (PenUp)
servoEndAngle = 125 # Lowest point of servo (PenDown)
PenMin = 10 # PenDown start position
PenMax = 200 # PenDown end position
# For program
counter = 0 # internal counter M1 (drawer lines)
count2 = 0 # internal counter M2 (pen pos)
count2max1 = 0
count2max2 = 0
X = 1 # how many runs / copies (test)
Y = 0 # internal counter HW test
ncopies = 1 # number of copies
string = "" # container for input
string2 = "" # container for input2
error = 99 # container for errors, should be set to 0 for ok.
# Timers
ptime = 0 # program start time
rtime1 = 0 # actual start print/plot time
rtime2 = 0 # actual end print/plot time
#
# Define subroutines
#
# Servo subroutine 1
#
def map( value, fromLow, fromHigh, toLow, toHigh):
return (toHigh-toLow)*(value-fromLow) / (fromHigh-fromLow) + toLow
# Servo subroutine 2
#
def servoWrite(angle): # make the servo rotate to specific angle (0-180 degrees)
if(angle<0):
angle = 0
elif(angle > 180):
angle = 180
p.ChangeDutyCycle(map(angle,0,180,SERVO_MIN_DUTY,SERVO_MAX_DUTY))
# map the angle to duty cycle and output it
def servoPenDown():
for dc in range(servoStartAngle, servoEndAngle, 1):# make servo rotate from servoStartAngle to servoEndAngle (deg)
servoWrite(dc) # Write to servo
# sleep(0.01)
# sleep(SERVO_delay1)
# p.start(0)
def servoPenUp():
for dc in range(servoEndAngle, servoStartAngle, -1):# make servo rotate from servoEndAngle to servoStartAngle (deg)
servoWrite(dc) # Write to servo
# sleep(0.01)
# sleep(SERVO_delay1)
# p.start(0)
#
# Print all switch stadia
#
def Pallsw():
global X, ncopies, counter, count2, retryC, Y
print()
print(" X ncopies counter count2 retryC error Y")
print(" ", X," ", ncopies," ", counter," ", count2," ", retryC," ", error," ", Y)
print()
print("Values of the 4 switches at end @", strftime("%H:%M:%S"))
print("Motor 1, switch 1: ", GPIO.input(m1sw1))
print("Motor 1, switch 2: ", GPIO.input(m1sw2))
print("Motor 2, switch 1: ", GPIO.input(m2sw1))
print("Motor 2, switch 2: ", GPIO.input(m2sw2))
#
# Motor 1 to start position (inwards, to start printing / plotting)
#
def M1startpos():
# set motor 1 rotation
GPIO.output(m1in1, True)
GPIO.output(m1in2, False)
# set counter back to 0
counter = 0
while GPIO.input(m1sw1):
pwm1.ChangeDutyCycle(DutyC)
sleep(setpulse)
pwm1.ChangeDutyCycle(0)
counter = counter + 1
# change motor rotation
GPIO.output(m1in1, False)
GPIO.output(m1in2, True)
# Motor 1 to end position (outwards, to load object)
#
def M1endpos():
# set motor 1 rotation
GPIO.output(m1in1, False)
GPIO.output(m1in2, True)
# set counter back to 0
counter = 0
while GPIO.input(m1sw2):
pwm1.ChangeDutyCycle(DutyC)
sleep(setpulse)
pwm1.ChangeDutyCycle(0)
counter = counter + 1
# change motor rotation
GPIO.output(m1in1, True)
GPIO.output(m1in2, False)
# Motor 2 change position (default rotation goes to home position, inwards)
# And automatically change when run
#
def M2changepos():
setState = GPIO.input(m2in1)
count2 = 0
# print(setState)
if setState:
while GPIO.input(m2sw1):
pwm2.ChangeDutyCycle(DutyC2)
sleep(setpulse)
pwm2.ChangeDutyCycle(0)
count2 += 1
GPIO.output(m2in1, False)
GPIO.output(m2in2, True)
else:
while GPIO.input(m2sw2):
pwm2.ChangeDutyCycle(DutyC2)
sleep(setpulse)
pwm2.ChangeDutyCycle(0)
count2 += 1
GPIO.output(m2in1, True)
GPIO.output(m2in2, False)
#
# Check Hardware, motors and their switches ( set to start position, place object )
#
def checkHW():
global error, retryC
Y = retryC
servoPenUp()
while error > 0:
if Y < 1: break
if GPIO.input(m1sw1):
if GPIO.input(m1sw2):
M1endpos() # if no end is reached
error = 1
else:
error = 0 # All ok
else:
if not GPIO.input(m1sw2): # both sw activated ??
print("Something is really wrong !! CHECK HARDWARE !!")
print()
print("It's related to switches of Motor 1, both are activated ??")
print("we continue, to try fix it ...")
print("Otherwise the program will end with an error code.")
error = 10
else:
M1endpos() # motor 1 not on right pos
error = 2
if GPIO.input(m2sw1):
if GPIO.input(m2sw2):
M2changepos() # if no end is reached
error = 3
else:
error = 0 # All ok
else:
if not GPIO.input(m2sw2): # both sw activated ??
print("Something is really wrong !! CHECK HARDWARE !!")
print()
print("It's related to switches of Motor 2, both are activated ??")
print("we continue, to try fix it ...")
print("Otherwise the program will end with an error code.")
error = 11
else:
M2changepos() # motor 2 not on right pos
error = 4
if not error == 0:
print(" Wait 1 sec ... ")
print("Status : ", error)
print()
sleep(1)
if error > 90: error -= 1
Y -= 1
if error > 0:
# Print all switches
#
Pallsw()
print()
print("REALLY ??? Whats gong on ???")
print("We tried ",retryC," times to start, but something keeps wrong.")
input("Something really is bad ... what ? ")
# We should jump to end of Program !!
#
# START Global code :
#
# get start time
ptime = strftime("%H:%M:%S")
print()
print("Start of DG's DC motor & switch test",Pversion," @ ", ptime)
print()
print("Check our hardware ...")
checkHW()
# Print all switches
#
Pallsw()
# Set error code to checkHW (otherwise it will skip next run)
#
error = 99
# M1 should now be open to load object to print /plot on,
# same as when plot is finished, with arm in closed position.
#
print()
print("Place object to plot on on loader...")
print()
string = input("What should be printed? ")
X = int(input('Please enter a positive number of copies: '))
if X < 1: X = 1
ncopies = X
#
# Check what we can do with the given file ....
# ... to be made
# if ok continue
# ... to be made
#
# Print all switches
#
Pallsw()
# Make sure the drawer is back inside machine before plot starts.
# When finished the drawing is out.
# Needs a button to close ? Or wait for next object to plot on ?
#
servoPenUp()
M1startpos()
#
print()
print("Each 'step' of the DC motors is not as accurate as the one Daniele Tartaglia made, i guess.")
print("For now i use the following settings for both motors. (which run on batteries, btw.)")
print()
print("setpulse = ", setpulse," shortest time to power motors, to 'make' steps (seconds)")
print("waittime = ", waittime," used during test & now delay for motors (seconds)")
print("DutyC = ", DutyC," actual dutycycle motor 1 (power)")
print("DutyC = ", DutyC2," actual dutycycle motor 2 (power)")
print()
print("So i use the DutyC to save the motors and adjust their power. (And their batteries.)")
# set start plot time
rtime1 = strftime("%H:%M:%S")
while X > 0:
while GPIO.input(m1sw2):
servoPenUp()
pwm1.ChangeDutyCycle(DutyC)
sleep(setpulse)
pwm1.ChangeDutyCycle(0)
setState = GPIO.input(m2in1)
count2 = 0
if PenMin <= counter <= (1.3*PenMax):
if setState:
while GPIO.input(m2sw1):
if PenMin <= count2 <= PenMax+5:
if -11 <= (count2-PenMin) <= (0.1*PenMax): servoPenUp()
if (0.1*PenMax)+1 <= (count2-PenMin) <= (0.2*PenMax): servoPenDown()
if (0.2*PenMax)+1 <= (count2-PenMin) <= (0.3*PenMax): servoPenUp()
if (0.3*PenMax)+1 <= (count2-PenMin) <= (0.4*PenMax): servoPenDown()
if (0.4*PenMax)+1 <= (count2-PenMin) <= (0.5*PenMax): servoPenUp()
if (0.5*PenMax)+1 <= (count2-PenMin) <= (0.6*PenMax): servoPenDown()
if (0.6*PenMax)+1 <= (count2-PenMin) <= (0.7*PenMax): servoPenUp()
if (0.7*PenMax)+1 <= (count2-PenMin) <= (0.8*PenMax): servoPenDown()
if (count2-PenMin) >= (0.9*PenMax): servoPenUp()
pwm2.ChangeDutyCycle(DutyC2)
sleep(setpulse*0.5)
pwm2.ChangeDutyCycle(0)
count2 += 1
if count2 > PenMax+6: servoPenUp()
GPIO.output(m2in1, False)
GPIO.output(m2in2, True)
count2max1 = count2
else:
while GPIO.input(m2sw2):
if PenMin <= count2 <= PenMax:
if -11 <= (count2-PenMin) <= (0.1*PenMax): servoPenUp()
if (0.1*PenMax)+1 <= (count2-PenMin) <= (0.2*PenMax): servoPenDown()
if (0.2*PenMax)+1 <= (count2-PenMin) <= (0.3*PenMax): servoPenUp()
if (0.3*PenMax)+1 <= (count2-PenMin) <= (0.4*PenMax): servoPenDown()
if (0.4*PenMax)+1 <= (count2-PenMin) <= (0.5*PenMax): servoPenUp()
if (0.5*PenMax)+1 <= (count2-PenMin) <= (0.6*PenMax): servoPenDown()
if (0.6*PenMax)+1 <= (count2-PenMin) <= (0.7*PenMax): servoPenUp()
if (0.7*PenMax)+1 <= (count2-PenMin) <= (0.8*PenMax): servoPenDown()
if (count2-PenMin) >= (0.9*PenMax): servoPenUp()
pwm2.ChangeDutyCycle(DutyC2)
sleep(setpulse*0.5)
pwm2.ChangeDutyCycle(0)
count2 += 1
if count2 > PenMax+1: servoPenUp()
GPIO.output(m2in1, True)
GPIO.output(m2in2, False)
count2max2 = count2
counter += 1
print(count2max1," ",count2max2)
# servoPenUp()
pwm1.ChangeDutyCycle(0)
pwm2.ChangeDutyCycle(0)
# Print should be finished now !
# set end plot time
#
rtime2 = strftime("%H:%M:%S")
# check arm position and move if wrong...
#
checkHW()
print()
# If more then 1 copy selected, move M1 to start-pos
#
if X>1:
print("Copy nr:", ncopies-(X-1)," is ready, please remove.")
string = input("Place next object and press ENTER to continue ... ")
M1startpos()
else:
print("Copy nr:", ncopies-(X-1),"(last) is ready, please remove.")
string = input("Press ENTER to stop ...")
X = X - 1
#
# END Global code :
#
M1startpos()
# stop motors & servo
#
pwm1.ChangeDutyCycle(0)
pwm2.ChangeDutyCycle(0)
#p.stop()
# Print all switches
#
Pallsw()
print()
print("Plot starttime : ", rtime1)
print("Plot endtime : ", rtime2)
print()
print("For ",ncopies," prints / plots.")
print()
print("Stop")
print()
# End of program, stop pwm and cleanup GPIO
#
p.stop()
pwm1.stop()
pwm2.stop()
GPIO.cleanup()
Inmiddels met laser hier werkend te zien.
DG.