Added color code driving, changed Plant order, Docu update

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()

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)

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)

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)
 

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)
 

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)

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)
 

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)
 

software/roboter/ev3/sensortest.py

@@ -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()
-    
-

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