diff --git a/py/src/drive.py b/py/src/drive.py index 0d12463..99eb933 100644 --- a/py/src/drive.py +++ b/py/src/drive.py @@ -7,6 +7,9 @@ class Drive(): + + angle = 90 + def __init__(self, motors: tuple, dimensions: tuple): leftMotor, rightMotor = motors wheelDiameter, axleTrack = dimensions @@ -22,11 +25,11 @@ def forward(self, speed): def backward(self, speed): self.driveBase.drive(-abs(speed), 0 ) - def left(self, speed): - self.driveBase.drive(speed, 90) + def setTurnAngle(self, value: int): + self.angle = value - def right(self, speed): - self.driveBase.turn(90) + def turn(self, direction): + self.driveBase.turn(self.angle * direction) def stop(self, brakeType): self.driveBase.drive(0,0) diff --git a/py/src/main.py b/py/src/main.py index 08eb296..8ef1815 100644 --- a/py/src/main.py +++ b/py/src/main.py @@ -19,18 +19,23 @@ wait(50) -# This program requires LEGO EV3 MicroPython v2.0 or higher. -# Click "Open user guide" on the EV3 extension tab for more information. +"""FIN JUSTER DISSE VERDIENE""" dimensions = ( 47, # Wheel diamter - 140 ) #Axle track + 145 ) #Axle track -motors = (Motor(port=Port.D), # Left motor for the drive base - Motor(port=Port.C)) # Right motor for the drive base -sensors = (Port.S3, # Lyssensor for å måle fargen på jorda - Port.S1) # Ultrasonisksensor for å sjekke for potensielle hindringer robotten må navigere unna +motors = (Motor(port=Port.C), # Left motor for the drive base + Motor(port=Port.D)) # Right motor for the drive base + +bucketPort = Port.B # Port spec for the bucket motor + +sensors = (Port.S4, # Lyssensor for å måle fargen på jorda + Port.S3) # Ultrasonisksensor for å sjekke for potensielle hindringer robotten må navigere unna robotDrive = Drive(motors, dimensions) # Robottens kjørefunksjonalitet robot = Robot(robotDrive, *sensors) # EV3 robot + +robot.initWaterBucketMotor(bucketPort) +robot.setBucketAngle(90) robot.start() \ No newline at end of file diff --git a/py/src/robot.py b/py/src/robot.py index e1d5d6c..16abc91 100644 --- a/py/src/robot.py +++ b/py/src/robot.py @@ -9,23 +9,47 @@ from drive import Drive from sensors import UltraSonic, Colour +from water_bucket import WaterBucket class Robot(): + + waterBucket = None + controlDistance = 100 + requiredReflection = 7 + nextDirection = 1 + def __init__(self, drive: Drive, colorPort, ultrsonicPort): self.hub = EV3Brick() self.drive = drive self.color = Colour(colorPort) self.ultrasonic = UltraSonic(ultrsonicPort) - + + def initWaterBucketMotor(self, motorPort, rotationDirection=Direction.CLOCKWISE): + self.waterBucket = WaterBucket(motorPort, rotationDirection) + def setControlDistance(self, value: int): + self.controlDistance = value + + def setRequiredReflection(self, value: int): + self.requiredReflection + + def setBucketSpeed(self, value: int): + self.waterBucket.setSpeed(value) + + def setBucketAngle(self, value: int): + self.waterBucket.setTargetAngle(value) + + + def say(self, string: str): + self.hub.speaker.say(string) + def start(self): self.speed = 150 self.drive.forward(self.speed) while True: wait(50) - - + print(self.waterBucket.motor.angle()) if self.ultrasonic.distanceLessThan(100): self.drive.stop(0) @@ -33,13 +57,29 @@ def start(self): wait(1000) self.drive.backward(self.speed) wait(1000) - self.drive.right(self.speed) + self.drive.turn(self.nextDirection) wait(1000) self.drive.forward(self.speed) wait(1000) - self.drive.right(self.speed) + self.drive.turn(self.nextDirection) wait(1000) + self.nextDirection *= -1 self.drive.forward(self.speed) + + + + + if self.color.reflectionGreaterThan(self.requiredReflection) and not self.waterBucket.checkIfOpen(): + if self.waterBucket is not None: + self.waterBucket.releaseWater() + print("Opening...") + else: + self.say("Releasing water") + elif not self.color.reflectionGreaterThan(self.requiredReflection) and self.waterBucket.checkIfOpen(): + if self.waterBucket is not None: + print("Closing...") + self.waterBucket.close() + def stop(self): diff --git a/py/src/sensors.py b/py/src/sensors.py index 84037fd..b08b717 100644 --- a/py/src/sensors.py +++ b/py/src/sensors.py @@ -9,11 +9,6 @@ from pybricks.nxtdevices import TemperatureSensor -""" -GAMMEL SENSOR FUNKSJONALITET -BEHOLDES FREM TIL NY FUNKSJONALITET ER IMPLEMENTERT -""" - """ NY SENSOR FUNKSJONALITET diff --git a/py/src/water_bucket.py b/py/src/water_bucket.py index a49031d..7be80a8 100644 --- a/py/src/water_bucket.py +++ b/py/src/water_bucket.py @@ -5,5 +5,32 @@ class WaterBucket: - def __init__(self): - pass \ No newline at end of file + + speed = 100 + angle = 45 + + def __init__(self, motorPort, rotationDirection): + self.motor = Motor(port=motorPort, positive_direction=rotationDirection) + self.isOpen = False + + def releaseWater(self): + if not self.isOpen: + self.motor.run_angle(self.speed, self.angle) + self.isOpen = True + + def close(self): + if self.isOpen: + self.motor.run_angle(self.speed, -self.getAngle()) + self.isOpen = False + + def setSpeed(self, value: int): + self.speed = value + + def setTargetAngle(self, value: int): + self.angle = value + + def checkIfOpen(self): + return self.isOpen + + def getAngle(self): + return self.motor.angle() \ No newline at end of file