Untitled

import sim
import matplotlib.pyplot as plt
from mindstorms import Motor, Direction, ColorSensor


class PIDController:
    def __init__(self, kp, ki, kd, setpoint):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.setpoint = setpoint
        self.previous_error = 0
        self.integral = 0

    def compute(self, measurement):
        error = self.setpoint - measurement
        self.integral += error
        derivative = error - self.previous_error
        output = (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
        self.previous_error = error
        return output


sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)


def show_image(image):
    plt.imshow(image)
    plt.show()


def is_red_detected(color_sensor):
    """
    Calculates the relative intensity of the red channel compared to
    other channels
    """
    red_ratio_threshold = 1.5
    red, green, blue = color_sensor.rgb()
    print(red, green, blue)
    red_intensity = red / (green + blue)

    return red_intensity > red_ratio_threshold


def is_blue_detected(color_sensor):
    """
       Calculates the relative intensity of the blue channel compared to
       other channels
       """
    blue_ratio_threshold = 1.5
    red, green, blue = color_sensor.rgb()
    blue_intensity = blue / (red + green)

    return blue_intensity > blue_ratio_threshold


def follow_line(color_sensor, left_motor, right_motor, pid_controller):
    reflection = color_sensor.reflection()
    print(f"Current Reflection: {reflection}")

    control_signal = pid_controller.compute(reflection)

    base_speed = 5
    left_speed = max(-100, min(100, base_speed - control_signal))
    right_speed = max(-100, min(100, base_speed + control_signal))

    left_motor.run(speed=int(left_speed))
    right_motor.run(speed=int(right_speed))


# MAIN CONTROL LOOP
if clientID != -1:
    print('Connected')
    left_motor = Motor(motor_port='A', direction=Direction.CLOCKWISE, clientID=clientID)
    right_motor = Motor(motor_port='B', direction=Direction.CLOCKWISE, clientID=clientID)
    color_sensor = ColorSensor(clientID=clientID)

    pid = PIDController(kp=0.1, ki=0.01, kd=0.005, setpoint=65)

    try:
        while True:
            follow_line(color_sensor, left_motor, right_motor, pid)
    except KeyboardInterrupt:
        print('Stopping robot')
        left_motor.stop()
        right_motor.stop()

    print('Program ended')
else:
    print('Failed connecting to remote API server')

# MAIN CONTROL LOOP