 ### 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(theta)
print(phi - theta)
print("\n")

# Calculate coordinate positions

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

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

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()

```