Image

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

Image Image

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.

Image Image Image

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:

Image

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