diff --git a/documentation/Robot.docx b/documentation/Robot.docx index dc6588f..bd36f52 100644 Binary files a/documentation/Robot.docx and b/documentation/Robot.docx differ diff --git a/software/roboter/ev3/drive_back.py b/software/roboter/ev3/drive_back.py index 8968f82..0c4d647 100644 --- a/software/roboter/ev3/drive_back.py +++ b/software/roboter/ev3/drive_back.py @@ -8,23 +8,31 @@ motorLeft = LargeMotor(OUTPUT_D) motorRight = LargeMotor(OUTPUT_A) motorUpDown = Motor(OUTPUT_C) motorLeftRight = Motor(OUTPUT_B) -ultraSensor = UltrasonicSensor(INPUT_4) -touchSensor = TouchSensor(INPUT_1) +sensorUltraSonic = UltrasonicSensor(INPUT_4) +sensorTouch = TouchSensor(INPUT_1) + +plantID = int(sys.argv[1]) + +# Set direction of arm rotation back to middle +if plantID % 2 == 0: + leftRight = 50 # rotating right +else: + leftRight = -50 # rotating left motorUpDown.on_for_seconds(SpeedPercent(-50), seconds=2.8) -motorLeftRight.on_for_degrees(SpeedPercent(int(sys.argv[1])), 400) +motorLeftRight.on_for_degrees(SpeedPercent(leftRight), 400) motorLeft.on(SpeedPercent(-70), block=False) motorRight.on(SpeedPercent(-70)) while True: - if ultraSensor.distance_centimeters < 10: + if sensorUltraSonic.distance_centimeters < 10: motorLeft.on(SpeedPercent(-25), block=False) motorRight.on(SpeedPercent(-25)) break -touchSensor.wait_for_pressed(timeout_ms=None, sleep_ms=10) +sensorTouch.wait_for_pressed(timeout_ms=None, sleep_ms=10) motorLeft.off() motorRight.off() \ No newline at end of file diff --git a/software/roboter/ev3/drive_plant.py b/software/roboter/ev3/drive_plant.py new file mode 100644 index 0000000..ab61c49 --- /dev/null +++ b/software/roboter/ev3/drive_plant.py @@ -0,0 +1,48 @@ +#!/usr/bin/env pybricks-micropython +''' +Script to drive robot to plant. +Plant number is passed as argument. +''' + + +from ev3dev2.motor import Motor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent +from ev3dev2.sensor.lego import ColorSensor +from ev3dev2.sensor import INPUT_2 +import sys + +motorRight = LargeMotor(OUTPUT_A) +motorLeft = LargeMotor(OUTPUT_D) +motorUpDown = Motor(OUTPUT_C) +motorLeftRight = Motor(OUTPUT_B) + +sensorColor = ColorSensor(INPUT_2) + +plantID = int(sys.argv[1]) + +# Set color according to plant number +if plantID == 1 or plantID == 2: + colorCode = ColorSensor.COLOR_RED +elif plantID == 3 or plantID == 4: + colorCode = ColorSensor.COLOR_GREEN +elif plantID == 5 or plantID == 6: + colorCode = ColorSensor.COLOR_BLUE + +# Set direction of arm rotation +if plantID % 2 == 0: + leftRight = -50 # rotating left +else: + leftRight = 50 # rotating right + + +motorRight.on(SpeedPercent(70), block=False) +motorLeft.on(SpeedPercent(70)) + +while True: + if sensorColor.color == colorCode: + break + +motorLeft.off() +motorRight.off() + +motorLeftRight.on_for_degrees(SpeedPercent(leftRight), 400) +motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8) \ No newline at end of file diff --git a/software/roboter/ev3/plant_1.py b/software/roboter/ev3/plant_1.py index 588f371..a8eb80d 100644 --- a/software/roboter/ev3/plant_1.py +++ b/software/roboter/ev3/plant_1.py @@ -10,6 +10,6 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=0.8) -motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8) diff --git a/software/roboter/ev3/plant_2.py b/software/roboter/ev3/plant_2.py index 9b50771..a33cc9d 100644 --- a/software/roboter/ev3/plant_2.py +++ b/software/roboter/ev3/plant_2.py @@ -10,7 +10,7 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=0.8) -motorLeftRight.on_for_degrees(SpeedPercent(50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8) diff --git a/software/roboter/ev3/plant_3.py b/software/roboter/ev3/plant_3.py index 8810f91..6df695d 100644 --- a/software/roboter/ev3/plant_3.py +++ b/software/roboter/ev3/plant_3.py @@ -10,7 +10,7 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.2) -motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8) diff --git a/software/roboter/ev3/plant_4.py b/software/roboter/ev3/plant_4.py index d7dd665..04304ad 100644 --- a/software/roboter/ev3/plant_4.py +++ b/software/roboter/ev3/plant_4.py @@ -10,6 +10,6 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.2) -motorLeftRight.on_for_degrees(SpeedPercent(50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3) diff --git a/software/roboter/ev3/plant_5.py b/software/roboter/ev3/plant_5.py index bc42413..1d9b6c9 100644 --- a/software/roboter/ev3/plant_5.py +++ b/software/roboter/ev3/plant_5.py @@ -10,7 +10,7 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.7) -motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3) diff --git a/software/roboter/ev3/plant_6.py b/software/roboter/ev3/plant_6.py index 1d9b6c9..bc42413 100644 --- a/software/roboter/ev3/plant_6.py +++ b/software/roboter/ev3/plant_6.py @@ -10,7 +10,7 @@ motorLeftRight = Motor(OUTPUT_B) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False) motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.7) -motorLeftRight.on_for_degrees(SpeedPercent(50), 400) +motorLeftRight.on_for_degrees(SpeedPercent(-50), 400) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3) diff --git a/software/roboter/ev3/sensortest.py b/software/roboter/ev3/sensortest.py deleted file mode 100644 index 66d0f63..0000000 --- a/software/roboter/ev3/sensortest.py +++ /dev/null @@ -1,21 +0,0 @@ -#!/usr/bin/env python3 -from ev3dev2.motor import LargeMotor, OUTPUT_A, SpeedPercent -from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor -from ev3dev2.sensor import INPUT_1, INPUT_4 - -m = LargeMotor(OUTPUT_A) -touch = TouchSensor(INPUT_1) -ultraSonic = UltrasonicSensor(INPUT_4) -ultraSonic.MODE_US_DIST_CM - -m.on(SpeedPercent(50)) - -while True: - if ultraSonic.distance_centimeters < 10: - m.on(SpeedPercent(25)) - break - -touch.wait_for_pressed(timeout_ms=None, sleep_ms=10) -m.off() - - diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py index 9129f96..a32d108 100644 --- a/software/roboter/raspy/functions.py +++ b/software/roboter/raspy/functions.py @@ -32,7 +32,9 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client): actionID (_type_): current ID of driving action client (mqtt.Client): current mqtt client for publishing """ + # FIXME Change to color code driving os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') + # os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}') print("Measuring Sensors") @@ -40,11 +42,7 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client): # measure_send_data(plantID, actionID, client) print("Robot driving home") - if plantID % 2 == 0: - leftRight = -50 #rotating left - else: - leftRight = 50 #rotating right - os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}') + os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}') print("Robot home") #TODO decide about robot occupied message