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
