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
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 wristRotateClockwise():
RPL.servoWrite(9, 10)
time.sleep(1)
RPL.servoWrite(9, 0)
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)
pot_swivel = RPL.analogRead(ppin_swivel) * 29 * math.pi / 18432
error_sw = abs(pot_swivel - a_swivel)
if pot_swivel > a_swivel and error_sw > max_error:
RPL.servoWrite(swivel_continuous, 2000)
elif pot_swivel < a_swivel and error_sw > max_error:
RPL.servoWrite(swivel_continuous, 1000)
elif error_sw < max_error:
RPL.servoWrite(swivel_continuous, 0)
key = getch() #read when a key is presses
speed = 1 #set starting step value
if key == 'd': #increase x value
x += 0.1 * speed
if test() == False:
x -= 0.1 * speed
elif key == 'a': #decrease x value
x -= 0.1 * speed
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)
