def elbow(dir, run_for = 1, speed = speed): if(dir): RPL.digitalWrite(elbow_dir, 1) else: RPL.digitalWrite(elbow_dir, 0) RPL.pwmWrite(elbow_pul, speed, speed * 2) time.sleep(run_for) stop()
def shoulder(dir, run_for = 1, speed = speed): if(dir): RPL.digitalWrite(shoulder_dir, 1) else: RPL.digitalWrite(shoulder_dir, 0) RPL.pwmWrite(shoulder_pul, speed, speed * 2) time.sleep(run_for) stop()
y_up() elif key.upper() == 'E': #increase z value z_up() if test() == False: z_down() elif key.upper() == 'Q': #decrease z value z_down() if test() == False: z_up() quit = False #for breaking the motor loop with the '1' key command try: #if not connected to a RoboPi, it can still run import RoboPiLib_pwm as RPL #to pull all files needed to run the motors RPL.RoboPiInit("/dev/ttyAMA0", 115200) #connect to RoboPi motor_speed = 500 max_error = 5 #max distance arm can be away from intended point (multiply by sqrt(3) to get max distance away) shoulder_pul = 1 #shoulder pulse pin shoulder_dir = 2 #shoulder direction pin elbow_pul = 3 #elbow pulse pin elbow_dir = 4 #elbow direction pin swivel_continuous = 1 #pin for swivel motor ppin_shoulder = 5 #pin number for shoulder potentiometer ppin_elbow = 6 #pin number for elbow potentiomenter ppin_swivel = 7 #pin number for swivel potentiomenter print "shoulder_pul", shoulder_pul
#Moves a stepper motor for 1 seconds import RoboPiLib_pwm as RPL import setup import time pul_pin = 1 dir_pin = 2 #direction can equal 0 or 1 for forwards or backwards direction = 0 #The smaller the speed, the faster the rotation speed = 500 RPL.pinMode(pul_pin, RPL.PWM) RPL.pinMode(direction_pin, RPL.OUTPUT) RPL.digitalWrite(dir_pin, direction) RPL.pwmWrite(pul_pin, speed, speed * 2) time.sleep(1) RPL.pwmWrite(pul_pin, 0, 0)
import RoboPiLib_pwm as RPL RPL.RoboPiInit("/dev/ttyAMA0", 115200) from time import sleep ppin = 7 #potentiomenter pin line while True: p1 = RPL.analogRead(ppin) print("p = ", int(pl)) sleep(0.05)
def wristGrasperOpen(): RPL.servoWrite(11, 10) time.sleep(1) RPL.servoWrite(11, 0)
def wristFlipUp(): RPL.servoWrite(10, 10) time.sleep(1) RPL.servoWrite(10, 0)
def wristRotateCounter(): RPL.servoWrite(9, 3000) time.sleep(1) RPL.servoWrite(9, 0)
def stop(): RPL.pwmWrite(shoulder_pul, 0, 10000) RPL.pwmWrite(elbow_pul, 0, 10000)
import RoboPiLib_pwm as RPL import time as time import math RPL.RoboPiInit("/dev/ttyAMA0", 115200) shoulder_pul = 6 shoulder_dir = 7 elbow_pul = 1 elbow_dir = 2 print shoulder_pul, " shoulder_pul" print shoulder_dir, " shoulder_dir" print elbow_pul, " elbow_pul" print elbow_dir, " elbow_dir" up = True down = False RPL.pinMode(shoulder_pul, RPL.PWM) RPL.pinMode(shoulder_dir, RPL.OUTPUT) RPL.pinMode(elbow_pul, RPL.PWM) RPL.pinMode(elbow_dir, RPL.OUTPUT) speed = 1000 def stop(): RPL.pwmWrite(shoulder_pul, 0, 10000) RPL.pwmWrite(elbow_pul, 0, 10000)
z = 0.0 #starting z value import math def test(): #function for angle domains d_three = (round(x, 2)**2 + round(y, 2)**2 + round(z, 2)**2)**0.5 if round(x, 1) == 0.0 and round(y, 1) == 0.0 and round(z, 1) == 0: return True if d_three > d_one + d_two or d_three < d_one - d_two: return False import RoboPiLib_pwm as RPL #to pull all the files needed to run the code RPL.RoboPiInit("/dev/ttyAMA0", 115200) #connect to RoboPi shoulder_pul = 1 #shoulde pulse pin shoulder_dir = 2 #shoulde direction pin elbow_pul = 3 #elbow pulse pin elbow_dir = 4 #elbow direction pin swivel_continuous = 1 #pin for swivel motor ppin_shoulder = 7 #pin number for shoulder potentiometer ppin_elbow = 8 #pin number for elbow potentiomenter ppin_swivel = 9 #pint number for swivel potentiomenter print 'shoulder_pul', shoulder_pul print 'shoulder_dir', shoulder_dir print 'elbow_pul', elbow_pul print 'elbow_dir', elbow_dir print 'swivel_continuous', swivel_continuous
import RoboPiLib_pwm as RPL #to pull all files needed to run the motors RPL.RoboPiInit("/dev/ttyAMA0", 115200) #connect to RoboPi import time pot_pin = 6 while True: pot_value = RPL.analogRead(pot_pin) print(pot_value) time.sleep(0.05)
def motor_runner( x, y, z): #sends signals to all the motors based on potentiometer readings sqd_one = d_one**2 sqd_two = d_two**2 d_three = math.sqrt((y**2) + (x**2)) elbow_value = math.acos( (sqd_one + sqd_two - ((y**2) + (x**2))) / (2 * d_one * d_two)) a_two = math.asin((d_two * math.sin(elbow_value) / d_three)) # angle between shoulder and wrist a_four = math.atan2(y, x) # angle between 0 line and wrist a_shoulder = round((a_four + a_two), 4) # shoulder angle reach_length = math.sqrt(x**2 + y**2 + z**2) #the momentary length of the arm a_elbow = round(abs(elbow_value - math.pi), 4) #the converted angle of the elbow # a_elbow = round(abs(elbow_value - math.pi), 4) #the converted angle of the elbow # try: if z > 0: a_swivel = round(math.acos(z / x), 4) elif z < 0: a_swivel = round(((math.pi / 2) + math.asin(math.fabs(z) / x)), 4) elif z == 0: a_swivel = round(math.pi / 2, 4) # a_swivel = round(math.asin(z / math.sqrt(x ** 2 + z ** 2)) + math.pi / 2, 4) #the swivel angle # except: # a_swivel = math.pi / 2 #the swivel angle when its angle doesn't matter try: # start w/ grasper if gopen == 0: RPL.servoWrite(gpin, 1400) elif gclose == 0: RPL.servoWrite(gpin, 1700) else: RPL.servoWrite(gpin, 0) if wup == 0: RPL.servoWrite(wpin, 1400) elif wdown == 0: RPL.servoWrite(wpin, 1700) else: RPL.servoWrite(wpin, 0) #move shoulder motor pot_shoulder = RPL.analogRead(ppin_shoulder) * 3 * math.pi / ( 2 * shoulder_range) - math.pi / 4 error_s = abs( pot_shoulder - a_shoulder) #how many degrees off the intended value the arm is calculated_error_s = error_s * d_two if calculated_error_s > max_error: if pot_shoulder > a_shoulder: RPL.digitalWrite(shoulder_dir, 0) #turn clockwise else: RPL.digitalWrite(shoulder_dir, 1) #turn counterclockwise RPL.pwmWrite(shoulder_pul, shoulder_motor_speed, shoulder_motor_speed * 2) else: RPL.pwmWrite(shoulder_pul, 0, shoulder_motor_speed * 2) #stops running while in range #move elbow motor pot_elbow = RPL.analogRead(ppin_elbow) * 3 * math.pi / ( 2 * elbow_range) - math.pi / 4 error_e = abs( pot_elbow - a_elbow) #how many degrees off the intended value the arm is calculated_error_e = error_e * d_two if calculated_error_e > max_error: if pot_elbow > a_elbow: RPL.digitalWrite(elbow_dir, 0) #turn clockwise else: RPL.digitalWrite(elbow_dir, 1) #turn counterclockwise RPL.pwmWrite(elbow_pul, elbow_motor_speed, elbow_motor_speed * 2) else: RPL.pwmWrite(elbow_pul, 0, elbow_motor_speed * 2) #stops running while in range #move swivel motor pot_swivel = RPL.analogRead(ppin_swivel) * 3 * math.pi / ( 2 * swivel_range) - math.pi / 4 error_sw = abs( pot_swivel - a_swivel) #how many degrees off the intended value the arm is if error_sw > max_error: if pot_swivel > a_swivel: RPL.servoWrite(swivel_continuous, 2000) #turn clockwise else: RPL.servoWrite(swivel_continuous, 1000) #turn counterclockwise else: RPL.servoWrite(swivel_continuous, 0) #stops running while in range if quit == True: #stop the motors when the code ends RPL.servoWrite(swivel_continuous, 0) #stops running while in range RPL.pwmWrite(elbow_pul, 0, elbow_motor_speed * 2) #stops running while in range RPL.pwmWrite(shoulder_pul, 0, shoulder_motor_speed * 2) #stops running while in range except: #to show the values of the motor arm print('[elbow, shoulder, swivel]:', [round(a_elbow, 4), round(a_shoulder, 4), round(a_swivel, 4)], '[Speed]:', [speed], '[x, y, z]:', [round(x, 2), round(y, 2), round(z, 2)], gopen, gclose)
def motor_runner( ): #sends signals to all the motors based on potentiometer readings while quit == False: reach_length = (x**2 + y**2 + z**2)**0.5 a_elbow = math.acos( round(((d_one**2 + d_two**2 - reach_length**2) / (2 * d_one * d_two)), 4)) a_shoulder = math.asin( round((d_two * math.sin(a_elbow) / reach_length), 2)) + math.asin( round((y / reach_length), 4)) a_swivel = math.atan2(round(x, 2), round(z, 2)) try: pot_shoulder = RPL.analogRead(ppin_shoulder) * 29 * math.pi / 18432 error_s = abs( pot_shoulder - a_shoulder ) #how many degrees off the intended value the arm is calculated_error_s = error_s * d_one if pot_shoulder > a_shoulder and calculated_error_s > max_error: RPL.digitalWrite(shoulder_dir, 0) #turn clockwise RPL.pwmWrite(shoulder_pul, motor_speed, motor_speed * 2) elif pot_shoulder < a_shoulder and calculated_error_s > max_error: RPL.digitalWrite(shoulder_dir, 1) #turn counterclockwise RPL.pwmWrite(shoulder_pul, motor_speed, motor_speed * 2) elif calculated_error_s < max_error: RPL.pwmWrite(shoulder_pul, 0, motor_speed * 2) #stops running while in range pot_elbow = RPL.analogRead(ppin_elbow) * 29 * math.pi / 18432 error_e = abs( pot_elbow - a_elbow) #how many degrees off the intended value the arm is calculated_error_e = error_e * d_two if pot_elbow > a_elbow and calculated_error_e > max_error: RPL.digitalWrite(elbow_dir, 1) #turn clockwise RPL.pwmWrite(elbow_pul, motor_speed, motor_speed * 2) elif pot_elbow < a_elbow and calculated_error_e > max_error: RPL.digitalWrite(elbow_dir, 0) #turn counterclockwise RPL.pwmWrite(elbow_pul, motor_speed, motor_speed * 2) elif calculated_error_e < max_error: RPL.pwmWrite(elbow_pul, 0, motor_speed * 2) #stops running while in range pot_swivel = RPL.analogRead(ppin_swivel) * 29 * math.pi / 18432 error_sw = abs( pot_swivel - a_swivel) #how many degrees off the intended value the arm is if pot_swivel > a_swivel and error_sw > max_error: RPL.servoWrite(swivel_continuous, 2000) #turn clockwise elif pot_swivel < a_swivel and error_sw > max_error: RPL.servoWrite(swivel_continuous, 1000) #turn counterclockwise elif error_sw < max_error: RPL.servoWrite(swivel_continuous, 0) #stops running while in range if quit == True: #stop the motors when the code ends RPL.servoWrite(swivel_continuous, 0) #stops running while in range RPL.pwmWrite(elbow_pul, 0, 10000) #stops running while in range RPL.pwmWrite(shoulder_pul, 0, 10000) #stops running while in range except: #to show the values of the motor arm import time time.sleep(1) print('[elbow, shoulder, swivel]:', [ round(a_elbow, 4), round(a_shoulder, 4), round(a_swivel, 4) ], '[Speed]:', [speed], '[x, y, z]:', [round(x, 2), round(y, 2), round(z, 2)])
def wristRotateClockwise(): RPL.servoWrite(9, 10) time.sleep(1) RPL.servoWrite(9, 0)
import RoboPiLib_pwm as RPL #to pull all files needed to run the motors RPL.RoboPiInit("/dev/ttyAMA0", 115200) #connect to RoboPi import time import math elbow_range = 775 #range of read values by elbow POT shoulder_range = 840 #range of read values by shoulder POT pot_pin_elbow = 6 pot_pin_shoulder = 7 print "Output given in degrees" print "Elbow pin:", pot_pin_elbow print "Shoulder pin:", pot_pin_shoulder time.sleep(3) while True: pot_value_elbow = RPL.analogRead(pot_pin_elbow) pot_value_shoulder = RPL.analogRead(pot_pin_shoulder) print(pot_value_elbow * 270 / elbow_range - 45 ) #to only use 180 degrees of the POT's range print(pot_value_elbow * 3 * math.pi / (2 * elbow_range) - math.pi / 4) time.sleep(0.1) print(pot_value_shoulder * 270 / shoulder_range - 45 ) #to only use 180 degrees of the POT's range print(pot_value_shoulder * 3 * math.pi / (2 * shoulder_range) - math.pi / 4) time.sleep(0.1)
import RoboPiLib_pwm as RPL #to pull all the files needed to run the motors RPL.RoboPiInit("/dev/ttyAMA0", 115200) #connect to RoboPi print 'intended angle (degrees):' a_shoulder = input('- ') print 'arm length (length):' arm_length = input('- ') print 'max error (length):' max_error = input('- ') motor_speed = 500 shoulder_pul = 1 shoulder_dir = 2 ppin = 7 print shoulder_pul, ' shoulder_pul' print shoulder_dir, ' shoulder_dir' print ppin, ' ppin' RPL.pinMode(shoulder_pul, RPL.PWM) #set shoulder_pul pin as a pulse-width modulation output RPL.pinMode(shoulder_dir, RPL.OUTPUT) #set shoulder_dir pin to an output and write 1 to it RPL.pwmWrite(shoulder_pul, 0, 1) while True: p1 = RPL.analogRead(ppin) * 145 / 512 - 55 print "p1: ", p1 error = abs( p1 - a_shoulder