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)), 2))
a_shoulder = math.asin(
round((d_two * math.sin(a_elbow) / reach_length), 2)) + math.asin(
round((y / reach_length), 2))
a_swivel = math.atan2(round(x, 2), round(z, 2))
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, 1) #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, 0) #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
(d_one**2 + d_two**2 - round(x, 2)**2 - round(y, 2)**2 -
round(z, 2)**2) / (2 * d_one * d_two))
a_shoulder = math.asin(
(d_two * math.sin(a_elbow) /
(round(x, 2)**2 + round(y, 2)**2 + round(z, 2)**2)**
0.5)) + math.atan2(round(y, 2),
(round(x, 2)**2 + round(z, 2)**2)**0.5)
a_swivel = math.atan2(round(x, 2), round(z, 2)) + math.pi / 2
else: #give motor values at (0, 0, 0)
a_elbow = x = y = z = 0.0
motor_speed = 500
max_error = 2
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, 1) #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, 0) #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)
pot_elbow = RPL.analogRead(ppin_elbow) * 29 * math.pi / 18432
error_e = abs(pot_elbow -
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)
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)
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
) * 3.14159265358979323846264338 / 180 #how many degrees off the intended value the arm is
calculated_error = error / arm_length
if p1 > a_shoulder and calculated_error > max_error:
RPL.digitalWrite(shoulder_dir, 1) #turn clockwise
RPL.pwmWrite(shoulder_pul, motor_speed, motor_speed * 2)
if p1 < a_shoulder and calculated_error > max_error:
RPL.digitalWrite(shoulder_dir, 0) #turn counterclockwise
RPL.pwmWrite(shoulder_pul, motor_speed, motor_speed * 2)
if calculated_error < max_error:
RPL.pwmWrite(shoulder_pul, 0, motor_speed * 2)
