Code Description

  • 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 stoped rotating. When the rotation angle difference is smaller than 10, the code reports the value of two rotation angles for further calculation.
  • 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
        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")
if __name__ == '__main__':