Image

Project 3: Trailer Control

Rui Shi & Ziyi Zhang

02/07/2018

Challenge 1: Maintaining constant position (aka “drive straight”)

Description

To maintain constant position and make the whole car move straight, the program keeps adjusting the direction when car is deviated. The gyro sensor can provide angle of the trailer. When the trailer is off correct direction, program will adjust direction of truck based on proportional control, and then the trailer can be back to normal direction.

Video Demo

Code:

#!/usr/bin/env python3
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('outD')
    sensor = ev3.GyroSensor('in2')
    sensor.mode='GYRO-ANG'
    kp = 10;
    bass = -200;
    loop = 1;
    while loop == 1: 
        angle = sensor.value();
        print(sensor.value())
        error = angle-2;
        vb = bass + error * kp;
        va = bass - error * kp;
        if va < -1000:
            va = -1000
        if va > 1000:
            va = 1000
        if vb < -1000:
            vb = -1000
        if vb > 1000:
            vb = 1000
        m1.run_forever(speed_sp = va);
        m2.run_forever(speed_sp = vb);
if __name__ == '__main__':
    main()

Challenge 2: Following a curve/path (aka “drive in a circle”)

Description

To achieve the goal of following a curve, the program keeps a certain angle between truck and trailer. Based on what we have in part in, we add another gyro sensor to measure angle of truck. With two gyro sensors, the program calculates difference between two angle values, keep the difference to a certain value with the proportional control in first part. We can change the radius by changing the setting difference between two angles.

Video Demo

Code:

#!/usr/bin/env python3
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('outD')
    #the trailer sensor
    sensor1 = ev3.GyroSensor('in2')
    #the cub sensor
    sensor2 = ev3.GyroSensor('in2')
    sensor1.mode='GYRO-ANG’
    sensor2.mode='GYRO-ANG'
    kp = 10;
    bass = -200;
    loop = 1;
    # prevent the error comes from the origin angle of two sensors are not 0
    angle_origin1 = sensor1.value();
    angle_origin2 = sensor2.value();
    while loop == 1: 
        angle1 = sensor1.value() - angle_origin1;
        angle2 = sensor2.value() - angle_origin2;
        diff = angle1 - angle2;
        error = diff - 30;
        #for smaller circle: error = diff - 60;
        vb = bass + error * kp;
        va = bass - error * kp;
        if va < -1000:
            va = -1000
        if va > 1000:
            va = 1000
        if vb < -1000:
            vb = -1000
        if vb > 1000:
            vb = 1000
        m1.run_forever(speed_sp = va);
        m2.run_forever(speed_sp = vb);
if __name__ == '__main__':
    main()2

Images of Our Car

Image Image Image Image