diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..00f2d38 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +__pycache__/ +*.pyc +.venv/ diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..f8f1a44 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,13 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations. + // Extension identifier format: ${publisher}.${name}. Example: vscode.csharp + + // List of extensions which should be recommended for users of this workspace. + "recommendations": [ + "lego-education.ev3-micropython" + ], + // List of extensions recommended by VS Code that should not be recommended for users of this workspace. + "unwantedRecommendations": [ + "ms-python.python" + ] +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..d933aeb --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Download and Run", + "type": "ev3devBrowser", + "request": "launch", + "program": "/home/robot/${workspaceRootFolderName}/main.py", + "interactiveTerminal": false + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..b0968c1 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +// Place your settings in this file to overwrite default and user settings. +{ + "files.eol": "\n", + "debug.openDebug": "neverOpen", + "python.linting.enabled": false, + "python.languageServer": "None" +} diff --git a/main.py b/main.py new file mode 100644 index 0000000..7681b0b --- /dev/null +++ b/main.py @@ -0,0 +1,115 @@ +#!/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, SoundFile, ImageFile, Align) +from pybricks.tools import print, wait, StopWatch + +import struct + +# Declare motors +left_motor = Motor(Port.B) +right_motor = Motor(Port.D) +#steer_motor = Motor(Port.A) +left = 0 +right = 0 + +# Dichiarazione device +device = EV3Brick() +device.speaker.set_volume(100, "_all_") +device.speaker.set_speech_options(language="it", speed=175, pitch=50) +device.speaker.say("Ciao Matteo. Mi sto preparando") + +# Dichiarazione sensore ultrasuoni +#ultrasuoni = UltrasonicSensor(Port.S3) + + +# Auto center steering wheels. +#steer_motor.run_until_stalled(250) +#steer_motor.reset_angle(90) +#steer_motor.run_target(300,0) + + +# A helper function for converting stick values (0 - 255) +# to more usable numbers (-100 - 100) +def scale(val, src, dst): + """ + Scale the given value from the scale of src to the scale of dst. + + val: float or int + src: tuple + dst: tuple + + example: print(scale(99, (0.0, 99.0), (-1.0, +1.0))) + """ + return (float(val-src[0]) / (src[1]-src[0])) * (dst[1]-dst[0])+dst[0] + + +# Open the Gamepad event file: +# /dev/input/event3 is for PS3 gamepad +# /dev/input/event4 is for PS4 gamepad +# look at contents of /proc/bus/input/devices if either one of them doesn't work. +# use 'cat /proc/bus/input/devices' and look for the event file. +infile_path = "/dev/input/event4" + +# open file in binary mode +in_file = open(infile_path, "rb") + +# Read from the file +# long int, long int, unsigned short, unsigned short, unsigned int +FORMAT = 'llHHI' +EVENT_SIZE = struct.calcsize(FORMAT) +event = in_file.read(EVENT_SIZE) + +device.speaker.say("Tutto fatto, adesso sono pronto") + +while event: + (tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event) + print("Event") + + #if ev_type == 1: # A button was pressed or released. + #if code == 310 and value == 0: #L1 + # steer_motor.reset_angle(steer_motor.angle()-5) + #if code == 311 and value == 1: #R1 + # left = scale(value, (0,255), (100,-100)) + # right = scale(value, (0,255), (100,-100)) + + #if code == 312 and value == 1: #L2 + # left = scale(value, (0,255), (100,-100)) + # right = scale(value, (0,255), (100,-100)) + #elif code == 312 and value == 0: #L2 + # left = scale(value, (0,255), (0,0)) + # right = scale(value, (0,255), (0,0)) + #elif code == 313 and value == 1: #R2 + # left = -scale(value, (0,255), (100,-100)) + # right = -scale(value, (0,255), (100,-100)) + # #device.speaker.beep(frequency=500, duration=-1) + #elif code == 313 and value == 0: #R2 rilasciato + # left = -scale(value, (0,255), (0,0)) + # right = -scale(value, (0,255), (0,0)) + # #device.speaker.beep(frequency=500, duration=0) + + if ev_type == 3: # Stick was moved + if code == 3: #right stick horizontal? + if value > 0: + left = -scale(value, (0,255), (100,-100)) + right = -scale(value, (0,255), (0,0)) + else: + right = -scale(value, (0,255), (100,-100)) + left = -scale(value, (0,255), (0,0)) + #left = scale(value, (0,255), (40, -40)) + if code == 1: # left stick vertical + left = -scale(value, (0,255), (100,-100)) + right = -scale(value, (0,255), (100,-100)) + + # Set motor voltages. + left_motor.dc(left) + right_motor.dc(right) + + # Track the steering angle + #steer_motor.track_target(left) + + # Finally, read another event + event = in_file.read(EVENT_SIZE) + +in_file.close() \ No newline at end of file