__Project 5: Robotic Arm Part II by Martin and Omar__

__Project 5: Robotic Arm Part II by Martin and Omar__

**Challenge 1: Calculating Motor Positions (based on End Effector Positions)**

In the first challenge, we were assigned to calculate the appropriate motor positions based on the previously calculated end effector positions of our code last week. We had to perform inverse kinematics which is a routine type of method in robotics to solve for joint parameters that provide a desired end effector positions. Our code is in fact incredibly accurate within +-2 degrees from the actual reading. Our calculated values are defined as the relative displacement that the motor rotates with respect to its zero. The best calculations are done in the first quadrant.

Below are pictures of the algebra needed to solve for each angle based on motor position (sorry Ipek, your work was greatly appreciated but we wanted to do them again for our own learning):

Below are the pictures displaying the angles read by the motor, the coordinates of the end effector, and the calculated angles of the motor based on those coordinates.

Here is the code:

#!/usr/bin/python3 import math import time def main(): print('CalculatingPositions.py') # Constant Definitions RUN_TIME = 30 INITBAR = 17 # centimeters SECONDBAR = 9 # centimeters # Connect to ev3 and motors ev3 = Device('this') endMotor = ev3.MediumMotor('outD') middleMotor = ev3.LargeMotor('outC') firstMotor = ev3.LargeMotor('outB') # Record starting positions initMidAngle = middleMotor.position initFirstAngle = firstMotor.position # Run for a certain period of time stop_time = time.time() + RUN_TIME while time.time() < stop_time: # Determine motor positions theta = firstMotor.position phi = theta - middleMotor.position # Print values in degrees print("Read Angle") print(theta) print(phi - theta) print("\n") # Calculate coordinate positions xPos = (INITBAR * math.cos(math.radians(theta))) + (SECONDBAR * math.cos(math.radians(phi))) yPos = (INITBAR * math.sin(math.radians(theta))) + (SECONDBAR * math.sin(math.radians(phi))) xPos = round(xPos,3) yPos = round(yPos,3) # Print out values in centimeters print("Position Values(x,y)") print(xPos) print(yPos) print("\n") sleep(0.5) # Calculate motor angles based on coordinate positions beta = math.atan2(xPos,yPos) R = math.sqrt( xPos**2 + yPos**2) theta2 = math.acos((R**2 - INITBAR**2 - SECONDBAR**2)/(2*INITBAR*SECONDBAR)) alpha = math.acos((INITBAR + SECONDBAR*math.cos(theta2))/(R)) theta1 = beta + alpha # Convert radians to degrees theta1 = math.degrees(theta1) theta1 = (theta1-90) * -1 theta2 = math.degrees(theta2) theta1 = round(theta1,3) theta2 = round(theta2,3) # Print values in degrees print("Calculated Angle") print(theta1) print(theta2) print("\n\n") if __name__ == '__main__': main()

**Challenge 2: Playing Back Sequence of End Effector Positions**

For challenge 2, we had to use a sequence of end effector positions which are simply (x,y) points and play back those positions. In our case, we decided to sequence the points in a circle to draw out. We mathematically indexed points of the circle using equations solving for the x and y components of the circle and looped those values. In one of our loops, the marker would be lifted up and when it reached the designated position it would drop. This would create a circle composed of points. In the other loop, we did not have the rise function, and simply drew out the circle.

Below is the finished circle when the pen was put down before moving and raised up only after finishing it's path:

Here is the dotted circle

Here is the code:

#!/usr/bin/python3 import math from time import sleep def dropPen( motor ): # Define constants for motor movement MOTOR_SP = 450 T_SLEEP = 2.55 # Lower pen motor.run_forever(speed_sp=MOTOR_SP) sleep(T_SLEEP) motor.stop() return def raisePen( motor ): # Define constants for motor movement MOTOR_SP = -450 T_SLEEP = 2.55 # Raise pen motor.run_forever(speed_sp=MOTOR_SP) sleep(T_SLEEP) motor.stop() return def circleCalc( xCenter, yCenter, r ): # Define function constants x = [] y = [] # Calculate x and y coordinates of a circle for i in range(0,20): deg = i * 360 / 20 x.append( r * math.cos( math.radians(deg) ) + xCenter ) y.append( r * math.sin( math.radians(deg) ) + yCenter ) # Return coordinates return x, y def main(): print('Test') # Constant Definitions RUN_TIME = 30 INITBAR = 17 # centimeters SECONDBAR = 9 # centimeters MOTOR_SP = 100 # ev3 Defintions ev3 = Device('this') endMotor = ev3.MediumMotor('outD') middleMotor = ev3.LargeMotor('outC') firstMotor = ev3.LargeMotor('outB') x, y = circleCalc( 18, 0, 3 ) dropPen( endMotor ) for i in range(0, len(x)): # Unpack variables xPos = x[i] yPos = y[i] # Calculate motor angles based on coordinate positions beta = math.atan2(xPos,yPos) R = math.sqrt( xPos**2 + yPos**2) theta2 = math.acos((R**2 - INITBAR**2 - SECONDBAR**2)/(2*INITBAR*SECONDBAR)) alpha = math.acos((INITBAR + SECONDBAR*math.cos(theta2))/(R)) theta1 = beta + alpha # Convert radians to degrees theta1 = math.degrees(theta1) theta1 = (theta1-90) * -1 theta2 = math.degrees(theta2) theta1 = round(theta1,3) theta2 = round(theta2,3) # Go to each position middleMotor.run_to_abs_pos(position_sp=theta2, speed_sp=MOTOR_SP, stop_action="hold") while middleMotor.is_running: sleep(0.1) firstMotor.run_to_abs_pos(position_sp=theta1, speed_sp=MOTOR_SP, stop_action="hold") while firstMotor.is_running: sleep(0.1) # dropPen( endMotor ) # sleep(0.1) # raisePen( endMotor) # sleep(0.1) # Release motors so they don't burn out raisePen( endMotor ) middleMotor.stop(stop_action="coast") firstMotor.stop(stop_action="coast") if __name__ == '__main__': main()