 ## Rui Shi & Ziyi Zhang

### Description

The goal of the project is to make a robotic arm which can hold a pen or something else and is able to write in a sheet. To construct such a system, we used two large motors, two gyros, a medium motor with a gear mounted on it to adjust the height of the "pen". To ensure each rotate part has the largest degree of freedom so that it can cover as much part of the sheet as possible, we connected three motors  in different horizontal level so they can rotate for almost 360°. Also, to avoid collision, one gyro was installed on the side of the motor rather than on top of it. Besides, we mounted a rod in the hole of the medium motor to make the arm capable to cover a larger area.    ### Challenge 1: Building the Robotic Arm

As shown in the pictures and video below, our motor is able to reach a much larger area than a 8.5*11 sheet. ### Challenge 2: Calculate End Effector Positions

For challenge two, the code need to use the Gyro sensor to report the rotation angles of both parts of the arm and we need to make sure the report angles is the final angle of two parts. To achieve this, I wrote an if sentence in the while loop to achieve the function that the code can report values only when both parts of the arm already rotated and stopped rotating. When the rotation angle difference is smaller than 10, the code reports the value of two rotation angles for further calculation.

In the video, we used a green pad with grids as reference. The distance between two bold lines are one inch. The following picture shows the coordinate system we used to measure positions of the pen. ### Challenge 3: Recording and Playing Back Positions

For challenge three, after getting the value of two rotation angles, we need to run the motor to return the arm to its origin shape. To achieve this function, first, we calculate the angle that two motors need to rotate. Then, we set two motors to rotate for that much in certain speed.

```#!/usr/bin/env python3
import math
def main():
ev3 = Device('this')

#Connect the  #1 motor to port A
#Connect 1the  #2 motor to port B
m1 = ev3.LargeMotor('outA')
m2 = ev3.LargeMotor('outB')

#Connect the  #1 sensor to port 1
#Connect 1the  #2 motor to port 3
sensor1 = ev3.GyroSensor('in1')
sensor2 = ev3.GyroSensor('in3')
sensor1.mode ='GYRO-ANG'
sensor2.mode ='GYRO-ANG'
angle1_origin = sensor1.value();
angle2_origin = sensor2.value();

#set the length of each part of the arm
l1 = 8
l2 = 9
loop = 1

#record the origin values of both sensors to prevent error
prev1 = sensor1.value()
prev2 = sensor2.value() - angle2_origin

#this part use sensors to record the rotate angles of both parts of the arm
while loop == 1:
angle1 = sensor1.value()
angle2 = sensor2.value() - angle2_origin
print("angle1 =%d, angle2 =%d" % (angle1, angle2))

#if both parts of the arm moved and already stoped moving, then break the loop and report the value of angle1 and angle2
if abs(angle1 - angle1_origin) > 10 and abs(angle2 - angle2_origin) > 10 and abs(prev1 - angle1) < 10 and abs(prev2 - angle2) < 10:
prev1 = angle1
prev2 = angle2
break
sleep(1)
prev1 = angle1
prev2 = angle2

#calculate angles that motor1 and motor2 should rotate, the angle motor2 should rotate equates to angle2 - angle1
theta1 = -(prev1)
theta2 = -(prev2 - prev1)
print("prev =%d, prev2 =%d" % (theta1, theta2))

#calculate x, y position
x = l1 * math.cos(prev1*2*3.14/360) + l2 * math.cos(prev2*2*3.14/360)
y = l1 * math.sin(prev1*2*3.14/360) + l2 * math.sin(prev2*2*3.14/360)
print("theta1 =%d, theta2 =%d" % (theta1, theta2))

#print x, y position
print("x =%d, y =%d" % (x, y))]

#run the motor to rotate back to its origin position
m1.run_to_rel_pos(position_sp=theta1, speed_sp=100, stop_action="hold")
m2.run_to_rel_pos(position_sp=theta2, speed_sp=100, stop_action="hold")
sleep(2)
m1.stop(stop_action="coast")
m2.stop(stop_action="coast")
if __name__ == '__main__':
main()```