Skip to content

Commit 9abe177

Browse files
michaelliaomichaelliao
committed
add tank
1 parent 7827c77 commit 9abe177

File tree

7 files changed

+244
-0
lines changed

7 files changed

+244
-0
lines changed

‎samples/micropython/tank/.vscode/extensions.json‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
{
2+
// See http://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
3+
// Extension identifier format: ${publisher}.${name}. Example: vscode.csharp
4+
5+
// List of extensions which should be recommended for users of this workspace.
6+
"recommendations": [
7+
"lego-education.ev3-micropython"
8+
],
9+
// List of extensions recommended by VS Code that should not be recommended for users of this workspace.
10+
"unwantedRecommendations": [
11+
"ms-python.python"
12+
]
13+
}

‎samples/micropython/tank/.vscode/launch.json‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
{
2+
// Use IntelliSense to learn about possible attributes.
3+
// Hover to view descriptions of existing attributes.
4+
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
5+
"version": "0.2.0",
6+
"configurations": [
7+
{
8+
"name": "Download and Run",
9+
"type": "ev3devBrowser",
10+
"request": "launch",
11+
"program": "/home/robot/${workspaceRootFolderName}/main.py"
12+
}
13+
]
14+
}

‎samples/micropython/tank/.vscode/settings.json‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
// Place your settings in this file to overwrite default and user settings.
2+
{
3+
"files.eol": "\n",
4+
"debug.openDebug": "neverOpen",
5+
"python.linting.enabled": false
6+
}

‎samples/micropython/tank/cannon.wav‎

138 KB
Binary file not shown.

‎samples/micropython/tank/devices.py‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
# list input devices
2+
3+
class InputDevice():
4+
def __init__(self):
5+
self.name = ''
6+
self.handler = ''
7+
8+
def __str__(self):
9+
return '<Input Device: name=%s, handler=%s>' % (self.name, self.handler)
10+
11+
def setName(self, name):
12+
if len(name) >= 2 and name.startswith('"') and name.endswith('"'):
13+
name = name[1:len(name)-1]
14+
self.name = name
15+
16+
def setHandler(self, handlers):
17+
for handler in handlers.split(' '):
18+
if handler.startswith('event'):
19+
self.handler = handler
20+
21+
def listDevices():
22+
devices = []
23+
with open('/proc/bus/input/devices', 'r') as f:
24+
device = None
25+
while True:
26+
s = f.readline()
27+
if s == '':
28+
break
29+
s = s.strip()
30+
if s == '':
31+
devices.append(device)
32+
device = None
33+
else:
34+
if device is None:
35+
device = InputDevice()
36+
if s.startswith('N: Name='):
37+
device.setName(s[8:])
38+
elif s.startswith('H: Handlers='):
39+
device.setHandler(s[12:])
40+
return devices
41+
42+
def detectJoystick(joystickNames):
43+
for device in listDevices():
44+
for joystickName in joystickNames:
45+
if joystickName in device.name:
46+
return '/dev/input/%s' % device.handler
47+
return None

‎samples/micropython/tank/joystick.py‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
# joystick control:
2+
3+
import struct
4+
5+
# define button code:
6+
7+
BUTTON_A = 305
8+
BUTTON_B = 304
9+
BUTTON_X = 307
10+
BUTTON_Y = 306
11+
BUTTON_PLUS = 313
12+
BUTTON_MINUS = 312
13+
BUTTON_START = 317
14+
BUTTON_HOME = 316
15+
16+
class JoyStick():
17+
def __init__(self, eventFile):
18+
self.eventFile = eventFile
19+
self.buttonHandler = None
20+
self.joyLeftHandler = None
21+
self.joyRightHandler = None
22+
23+
def setButtonHandler(self, buttonHandler):
24+
self.buttonHandler = buttonHandler
25+
26+
def setJoyLeftHandler(self, joyLeftHandler):
27+
self.joyLeftHandler = joyLeftHandler
28+
29+
def setJoyRightHandler(self, joyRightHandler):
30+
self.joyRightHandler = joyRightHandler
31+
32+
def startLoop(self):
33+
FORMAT = 'llHHI'
34+
EVENT_SIZE = struct.calcsize(FORMAT)
35+
with open(self.eventFile, 'rb') as infile:
36+
lx, ly, rx, ry = 0, 0, 0, 0
37+
while True:
38+
event = infile.read(EVENT_SIZE)
39+
_, _, t, c, v = struct.unpack(FORMAT, event)
40+
if t == 1 and v == 1:
41+
# button pressed:
42+
if self.buttonHandler:
43+
if not self.buttonHandler(c):
44+
return
45+
if t == 3:
46+
if c == 0 and self.joyLeftHandler:
47+
# left stick & horizontal:
48+
lx = v - 32768
49+
self.joyLeftHandler(lx, ly)
50+
elif c == 1 and self.joyLeftHandler:
51+
# left stick & vertical:
52+
ly = v - 32768
53+
self.joyLeftHandler(lx, ly)
54+
elif c == 3 and self.joyRightHandler:
55+
# right stick & horizontal:
56+
rx = v - 32768
57+
self.joyRightHandler(rx, ry)
58+
elif c == 4 and self.joyRightHandler:
59+
# right stick & vertical:
60+
ry = v - 32768
61+
self.joyRightHandler(rx, ry)

‎samples/micropython/tank/main.py‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
#!/usr/bin/env pybricks-micropython
2+
3+
import struct, threading
4+
5+
from pybricks import ev3brick as brick
6+
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor)
7+
from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align)
8+
from pybricks.tools import print, wait, StopWatch
9+
from pybricks.robotics import DriveBase
10+
11+
from devices import detectJoystick
12+
from joystick import JoyStick, BUTTON_A, BUTTON_X
13+
14+
SPEED = 100
15+
STEERING = 90
16+
17+
STATUS_STOPPED = 0
18+
STATUS_FORWARD = 1
19+
STATUS_BACKWARD = 2
20+
STATUS_STEERING = 3
21+
22+
COLORS = (None, Color.GREEN, Color.RED, Color.YELLOW)
23+
24+
class Driver():
25+
def __init__(self, leftMotor, rightMotor, diameter, axle):
26+
self.driver = DriveBase(leftMotor, rightMotor, diameter, axle)
27+
self.x = 0
28+
self.y = 0
29+
self.speed = 0
30+
self.steering = 0
31+
32+
def drive(self, speed, steering):
33+
self.speed = speed
34+
self.steering = steering
35+
if self.speed == 0:
36+
self.driver.stop()
37+
else:
38+
self.driver.drive(self.speed, self.steering)
39+
40+
class Robot():
41+
def __init__(self, leftMotor, rightMotor, topMotor, diameter, axle, maxSpeed=300, maxSteering=180, port=Port.S4):
42+
self.driver = Driver(leftMotor, rightMotor, diameter, axle)
43+
self.cannon = topMotor
44+
self.ultrasonic = UltrasonicSensor(port)
45+
self.speedStep = 32767 // maxSpeed
46+
self.steeringStep = 32767 // maxSteering
47+
self.active = True
48+
49+
def drive(self, x, y):
50+
# map y (-32768 ~ +32767) to speed (+maxSpeed ~ -maxSpeed):
51+
speed = -y // self.speedStep
52+
# map x (-32768 ~ +32767) to steering (-maxSteering ~ +maxSteering):
53+
steering = x // self.steeringStep
54+
self.driver.drive(speed, steering)
55+
56+
def target(self, x):
57+
self.cannon.run(-x // 327)
58+
59+
def fire(self):
60+
brick.sound.file('cannon.wav')
61+
62+
def inactive(self):
63+
self.active = False
64+
self.drive(0, 0)
65+
brick.sound.beep()
66+
67+
def autoStopLoop(robot):
68+
while robot.active:
69+
if robot.ultrasonic.distance() < 200:
70+
robot.drive(0, 0)
71+
wait(100)
72+
73+
def main():
74+
brick.sound.beep()
75+
joystickEvent = detectJoystick(['Controller'])
76+
if joystickEvent:
77+
robot = Robot(Motor(Port.D), Motor(Port.A), Motor(Port.B), 55, 200)
78+
t = threading.Thread(target=autoStopLoop, args=(robot,))
79+
t.start()
80+
81+
def onButtonPressed(code):
82+
if code == BUTTON_X:
83+
robot.inactive()
84+
return False
85+
if code == BUTTON_A:
86+
robot.fire()
87+
return True
88+
89+
def onLeftJoyChanged(x, y):
90+
robot.drive(x, y)
91+
92+
def onRightJoyChanged(x, y):
93+
robot.target(x)
94+
95+
joystick = JoyStick(joystickEvent)
96+
joystick.setButtonHandler(onButtonPressed)
97+
joystick.setJoyLeftHandler(onLeftJoyChanged)
98+
joystick.setJoyRightHandler(onRightJoyChanged)
99+
joystick.startLoop()
100+
else:
101+
brick.sound.beep()
102+
103+
main()

0 commit comments

Comments
 (0)