Prosjekt4_Kode

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
from time import sleep

SPEED = 200
PROPORTIONAL_GAIN = 0.8

ev3 = EV3Brick()

class ChampRobot():
    def __init__(self):
        leftMotor = Motor(Port.A)
        rightMotor = Motor(Port.D)
        self.robot = DriveBase(leftMotor, rightMotor,
        wheel_diameter= 56, axle_track = 114)
        self.right_color_sensor = ColorSensor(Port.S4)
        self.left_color_sensor = ColorSensor(Port.S3)
        self.pushbutton_right = TouchSensor(Port.S1)
        self.sensor_calibration_white = False
        self.sensor_calibration_black = False

    def run(self):
        self.colorCalibration()
        
        while True:
            # Read current sensor values
            left_reflection = self.left_color_sensor.reflection()
            right_reflection = self.right_color_sensor.reflection()
            
            # Calculate the error for each sensor (positive = darker/blacker, negative = lighter/whiter)
            left_error = left_reflection - self.threshold_left
            right_error = right_reflection - self.threshold_right
            
            # Use the DIFFERENCE between sensors to steer (keeps line centered between sensors)
            # If left sees more black than right, turn left (negative turn_rate)
            # If right sees more black than left, turn right (positive turn_rate)
            turn_rate = (left_error - right_error) * PROPORTIONAL_GAIN
            
            # Display current readings on screen (updates every ~100ms)
            if self.robot.distance() % 50 < 10:  # Update display occasionally to avoid slowing down
                ev3.screen.clear()
                ev3.screen.print("L: " + str(left_reflection) + " R: " + str(right_reflection))
                ev3.screen.print("Turn: " + str(int(turn_rate)))
            
            self.robot.drive(SPEED, turn_rate)
            wait(10)  # Small delay to prevent tight loop
            


   # def main(self):
        #self.robot.drive(SPEED, self.turn_rate)
        

    def colorCalibration(self):
        # Calibrate White
        while not self.sensor_calibration_white: 
            ev3.screen.clear()
            ev3.screen.print("Calibrate White")
            ev3.screen.print("L: " + str(self.left_color_sensor.reflection()))
            ev3.screen.print("R: " + str(self.right_color_sensor.reflection()))
            ev3.screen.print("Push to save")
            wait(100)
            
            if self.pushbutton_right.pressed():
                # Wait for button release to avoid double-press
                while self.pushbutton_right.pressed():
                    wait(10)
                
                self.white_left = self.left_color_sensor.reflection()
                self.white_right = self.right_color_sensor.reflection()
                
                ev3.screen.clear()
                ev3.screen.print("White saved!")
                ev3.screen.print("L: " + str(self.white_left))
                ev3.screen.print("R: " + str(self.white_right))
                ev3.speaker.beep()
                wait(1000)
                
                self.sensor_calibration_white = True
    
        # Calibrate Black
        while not self.sensor_calibration_black: 
            ev3.screen.clear()
            ev3.screen.print("Calibrate Black")
            ev3.screen.print("L: " + str(self.left_color_sensor.reflection()))
            ev3.screen.print("R: " + str(self.right_color_sensor.reflection()))
            ev3.screen.print("Push to save")
            wait(100)
            
            if self.pushbutton_right.pressed():
                # Wait for button release to avoid double-press
                while self.pushbutton_right.pressed():
                    wait(10)
                
                self.black_left = self.left_color_sensor.reflection()
                self.black_right = self.right_color_sensor.reflection()
                
                ev3.screen.clear()
                ev3.screen.print("Black saved!")
                ev3.screen.print("L: " + str(self.black_left))
                ev3.screen.print("R: " + str(self.black_right))
                ev3.speaker.beep()
                wait(1000)
                
                self.sensor_calibration_black = True
        
        # Calculate thresholds and display them
        if self.sensor_calibration_black and self.sensor_calibration_white:
            self.threshold_left = (self.white_left + self.black_left) / 2
            self.threshold_right = (self.white_right + self.black_right) / 2
            
            ev3.screen.clear()
            ev3.screen.print("Thresholds:")
            ev3.screen.print("L: " + str(int(self.threshold_left)))
            ev3.screen.print("R: " + str(int(self.threshold_right)))
            ev3.screen.print("Starting...")
            ev3.speaker.beep()
            wait(5000)

        
            
champrobot = ChampRobot()
champrobot.run()