Browse Source

Added color code driving, changed Plant order, Docu update

master
waldluis 1 year ago
parent
commit
830d662e08

BIN
documentation/Robot.docx View File


+ 13
- 5
software/roboter/ev3/drive_back.py View File

motorRight = LargeMotor(OUTPUT_A) motorRight = LargeMotor(OUTPUT_A)
motorUpDown = Motor(OUTPUT_C) motorUpDown = Motor(OUTPUT_C)
motorLeftRight = Motor(OUTPUT_B) 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) 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) motorLeft.on(SpeedPercent(-70), block=False)
motorRight.on(SpeedPercent(-70)) motorRight.on(SpeedPercent(-70))


while True: while True:
if ultraSensor.distance_centimeters < 10:
if sensorUltraSonic.distance_centimeters < 10:
motorLeft.on(SpeedPercent(-25), block=False) motorLeft.on(SpeedPercent(-25), block=False)
motorRight.on(SpeedPercent(-25)) motorRight.on(SpeedPercent(-25))
break break




touchSensor.wait_for_pressed(timeout_ms=None, sleep_ms=10)
sensorTouch.wait_for_pressed(timeout_ms=None, sleep_ms=10)
motorLeft.off() motorLeft.off()
motorRight.off() motorRight.off()

+ 48
- 0
software/roboter/ev3/drive_plant.py View File

#!/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)

+ 1
- 1
software/roboter/ev3/plant_1.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=0.8) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8)

+ 1
- 1
software/roboter/ev3/plant_2.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=0.8, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=0.8) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8)



+ 1
- 1
software/roboter/ev3/plant_3.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.2) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=2.8)



+ 1
- 1
software/roboter/ev3/plant_4.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.2, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.2) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3)

+ 1
- 1
software/roboter/ev3/plant_5.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.7) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3)



+ 1
- 1
software/roboter/ev3/plant_6.py View File

motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False) motorRight.on_for_seconds(SpeedPercent(70), seconds=2.7, block=False)
motorLeft.on_for_seconds(SpeedPercent(70), seconds=2.7) 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) motorUpDown.on_for_seconds(SpeedPercent(50), seconds=3)



+ 0
- 21
software/roboter/ev3/sensortest.py View File

#!/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()


+ 3
- 5
software/roboter/raspy/functions.py View File

actionID (_type_): current ID of driving action actionID (_type_): current ID of driving action
client (mqtt.Client): current mqtt client for publishing 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/plant_{plantID}.py')
# os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')


print("Measuring Sensors") print("Measuring Sensors")


# measure_send_data(plantID, actionID, client) # measure_send_data(plantID, actionID, client)


print("Robot driving home") 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") print("Robot home")


#TODO decide about robot occupied message #TODO decide about robot occupied message

Loading…
Cancel
Save