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)