Untitled

# GENERAL PURPOSE ( Color Maze )
#!usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, GyroSensor, ColorSensor)
from pybricks.parameters import (Port, Color)
from pybricks.robotics import DriveBase

# OBJECTS
robot = DriveBase(Motor(Port.A), Motor(Port.B), wheel_diameter = 142.24, axle_track = 152)
left_wheel = Motor(Port.A)
right_wheel = Motor(Port.B)
color_sensor = ColorSensor(Port.S1)
gyro_sensor = GyroSensor(Port.S3)
ev3_brick = EV3Brick()

# FUNCTIONS
## This function allows for a precise turn.
def turn(turn_value):
    gyro_sensor.reset_angle(gyro_sensor.angle())
    
    if turn_value > 0:
        while not gyro_sensor.angle() > (turn_value - 1):
            last_error = 0
            kp = 2
            kd = 0.5
            
            error = turn_value - gyro_sensor.angle()
            derivative = error - last_error
            
            proportional_gain = error * kp
            derivative_gain = derivative * kd
            
            correction = proportional_gain + derivative_gain
            left_wheel.run(correction + 5)
            right_wheel.run(-correction - 5)
            
        right_wheel.hold()
        left_wheel.hold()
        robot.drive(0, 0)

    else:
        while not gyro_sensor.angle() < (turn_value + 1):
            last_error = 0
            kp = -2
            kd = -0.5
            
            error = turn_value - gyro_sensor.angle()
            derivative = error - last_error
            
            proportional_gain = error * kp
            derivative_gain = derivative * kd
            
            correction = proportional_gain + derivative_gain
            left_wheel.run(-correction - 5)
            right_wheel.run(correction + 5)

        right_wheel.hold()
        left_wheel.hold()
        robot.drive(0, 0)

## This function allows a precise move forward and the brain of this program
def gyrostraight(colors):
    gyro_sensor.reset_angle(gyro_sensor.angle())
    angle_to_compare_to = gyro_sensor.angle()
    
    left_wheel.reset_angle()
    right_wheel.reset_angle()
            
    for color in colors:
        if color == Color.GREEN:
            integral = 0
            last_error = 0
                
            kp = 2
            ki = 0.09
            kd = 0.5
            
            if color_sensor.color() == Color.GREEN:
                while color_sensor.color() == Color.GREEN:
                    error = angle_to_compare_to - gyro_sensor.angle()
                    integral += error
                    derivative = error - last_error
                    
                    proportional_gain = error * kp
                    integral_gain = integral * ki
                    derivative_gain = derivative * kd
                    
                    correction = proportional_gain + integral_gain + derivative_gain
                    robot.drive(100, correction)
                    
                robot.drive(0, 0)
                left_wheel.hold()
                right_wheel.hold()
            else:
                while not robot.distance() >= 80:
                    error = angle_to_compare_to - gyro_sensor.angle()
                    integral += error
                    derivative = error - last_error
                    
                    proportional_gain = error * kp
                    integral_gain = integral * ki
                    derivative_gain = derivative * kd
                    
                    correction = proportional_gain + integral_gain + derivative_gain
                    robot.drive(100, correction)
                
                robot.drive(0, 0)
                left_wheel.hold()
                right_wheel.hold() 
            
            # while color_sensor.color() == Color.GREEN:                
            #     error = angle_to_compare_to - gyro_sensor.angle()
            #     integral += error
            #     derivative = error - last_error
                    
            #     proportional_gain = error * kp
            #     integral_gain = integral * ki
            #     derivative_gain = derivative * kd

            #     correction = proportional_gain + integral_gain + derivative_gain
            #     robot.drive(100, correction)
                    
            #     error = last_error
                
            #     if color_sensor.color() != Color.GREEN:
            #         while not robot.distance() >= 40:
            #             error = angle_to_compare_to - gyro_sensor.angle()
            #             integral += error
            #             derivative = error - last_error
                            
            #             proportional_gain = error * kp
            #             integral_gain = integral * ki
            #             derivative_gain = derivative * kd
                            
            #             correction = proportional_gain + integral_gain + derivative_gain
            #             robot.drive(100, correction)
                            
            #             error - last_error
            
            # while not robot.distance() >= 40:                
            #     error = angle_to_compare_to - gyro_sensor.angle()
            #     integral += error
            #     derivative = error - last_error
                        
            #     proportional_gain = error * kp
            #     integral_gain = integral * ki
            #     derivative_gain = derivative * kd

            #     correction = proportional_gain + integral_gain + derivative_gain
            #     robot.drive(100, correction)
                        
            #     error = last_error
                    
            robot.drive(0, 0)
            left_wheel.hold()
            right_wheel.hold()
                
        elif color == Color.RED:
            turn(-90)
            while not robot.distance() >= 80:                
                error = angle_to_compare_to - gyro_sensor.angle()
                integral += error
                derivative = error - last_error
                    
                proportional_gain = error * kp
                integral_gain = integral * ki
                derivative_gain = derivative * kd

                correction = proportional_gain + integral_gain + derivative_gain
                robot.drive(100, correction)
                    
                error = last_error
            
            robot.drive(0, 0)
            left_wheel.hold()
            right_wheel.hold()
            
        elif color == Color.BLUE:
            turn(90)
            while not robot.distance() >= 80:                
                error = angle_to_compare_to - gyro_sensor.angle()
                integral += error
                derivative = error - last_error
                    
                proportional_gain = error * kp
                integral_gain = integral * ki
                derivative_gain = derivative * kd

                correction = proportional_gain + integral_gain + derivative_gain
                robot.drive(100, correction)
                    
                error = last_error
            
            robot.drive(0, 0)
            left_wheel.hold()
            right_wheel.hold()
  
# PROGRAM
while True:
    gyrostraight((Color.GREEN, Color.RED, Color.BLUE))