Project 5 Annalisa and Raven

Description: We added a third motor to the end of our robotic arm from project 4. This motor rotates 90 degrees once the end effector is at its desired first position. The arm moves to the specified coordinates while the pen is drawing. Trigonometry calculations were used from Ipek's notes in order to calculate what we call "new motor positions" in our code.

Image Image

.

.

.

.

Challenge 1:

 #!/usr/bin/env python3 

def main():
    import math
    motor_pos1 = None
    motor_pos2 = None
    x_a = None
    y_a = None
    x_b = None
    y_b = None
    length1 = 12
    length2 = 13
    x_array = [1, 17]
    y_array = [20, 7]
    new_mot_pos1_array = [0, 0, 0, 0, 0, 0]
    new_mot_pos2_array = [0, 0, 0, 0, 0, 0]
    i = 0
    speed1 = 10*10.5
    speed2 = 20*10.5
    j = 0

    # set up the motor
    ev3 = Device('this')
    m = ev3.LargeMotor('outA')
    n = ev3.LargeMotor('outB')
    p = ev3.LargeMotor('outC')

    for count in range(len(x_array)):
        x_b = x_array[i]
        y_b = y_array[i]
        print('End Effector X:' + str(x_b))
        print('End Effector Y:' + str(y_b))
        sleep(2)
        r_squared = x_b**2 +y_b**2
        print('r:' + str(r_squared))

    #calculated motor 1 position
        cos_alpha = (length2**2 - (length1**2 + (r_squared)))/((-2)*length1*(math.sqrt(r_squared)))
        print('cos_alpha:' + str(cos_alpha))
        alpha = math.atan((math.sqrt(1- cos_alpha**2))/cos_alpha)
        alpha = math.degrees(alpha)
        print('alpha:' + str(alpha)) 
        beta = math.atan(y_b/x_b)
        beta = math.degrees(beta)
        print('beta:' + str(beta))
        new_motor_pos1 = alpha + beta
        new_mot_pos1_array[i] = new_motor_pos1
        print('New Motor 1 Position:' + str(new_motor_pos1))

    #calculated motor 2 position
        cos_2 = (r_squared - (length1**2 + length2**2))/(-2*length1*length2)
        print('cos_2:' + str(cos_2))
        sqrt_var = (math.sqrt(1-cos_2**2))/cos_2
        print('sqrtvar:' + str(sqrt_var))
        new_motor_pos2 = math.atan(sqrt_var) 
        new_motor_pos2 = math.degrees(new_motor_pos2)
        new_mot_pos2_array[i] = new_motor_pos2
        print('New Motor 2 Position:' + str(new_motor_pos2))

        #new end effector position
        x_a = length1 * math.cos(new_motor_pos1 / 180.0 * math.pi)
        y_a = length1 * math.sin(new_motor_pos1 / 180.0 * math.pi)
        x_b = x_a + length2 * math.cos((new_motor_pos1+new_motor_pos2) / 180.0 * math.pi)
        y_b = y_a + length2 * math.sin((new_motor_pos1+new_motor_pos2) / 180.0 * math.pi)
        print('New End Effector X:' + str(x_b))
        print('New End Effector Y:' + str(y_b))
        i = i + 1
        print('i = ' + str(i))
    pass

if __name__ == '__main__':
    main()
.
.

Challenge 2: we added this to the bottom of our challenge 1 code:

   for count in range(len(x_array)):
         # run to position 1
        print('Going to Position 1: ' + str(new_mot_pos1_array[j]))
        m.run_to_abs_pos(position_sp=new_mot_pos1_array[j], speed_sp=speed1, stop_action="hold")
         # run to position 2
        print('Going to Position 2: ' + str(new_mot_pos2_array[j]))
        n.run_to_abs_pos(position_sp=new_mot_pos2_array[j], speed_sp=speed1, stop_action="hold")
        # wait till done moving (keep checking)
        while n.is_running:
            sleep(0.1)
        j = j + 1
        sleep(1)
        # After moving to the first position, put the pen down 90 degrees and keep it there
        p.run_to_abs_pos(position_sp= -90, speed_sp=speed2, stop_action="hold")
        sleep(.5)
    pass
# OUTSIDE OF THE LOOP WE'LL PULL THE PEN BACK UP 
    print('pen back up')
    p.run_to_abs_pos(position_sp= 0, speed_sp=speed1, stop_action="hold")

# "relax" the motor at end of the program
    m.stop(stop_action="coast")
    n.stop(stop_action="coast")
    p.stop(stop_action="coast")

if __name__ == '__main__':
    main()