forked from michaelliao/learn-python3
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmain.py
More file actions
103 lines (83 loc) · 3.01 KB
/
main.py
File metadata and controls
103 lines (83 loc) · 3.01 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#!/usr/bin/env pybricks-micropython
import struct, threading
from pybricks import ev3brick as brick
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
from pybricks.robotics import DriveBase
from devices import detectJoystick
from joystick import JoyStick, BUTTON_A, BUTTON_X
SPEED = 100
STEERING = 90
STATUS_STOPPED = 0
STATUS_FORWARD = 1
STATUS_BACKWARD = 2
STATUS_STEERING = 3
COLORS = (None, Color.GREEN, Color.RED, Color.YELLOW)
class Driver():
def __init__(self, leftMotor, rightMotor, diameter, axle):
self.driver = DriveBase(leftMotor, rightMotor, diameter, axle)
self.x = 0
self.y = 0
self.speed = 0
self.steering = 0
def drive(self, speed, steering):
self.speed = speed
self.steering = steering
if self.speed == 0:
self.driver.stop()
else:
self.driver.drive(self.speed, self.steering)
class Robot():
def __init__(self, leftMotor, rightMotor, topMotor, diameter, axle, maxSpeed=300, maxSteering=180, port=Port.S4):
self.driver = Driver(leftMotor, rightMotor, diameter, axle)
self.cannon = topMotor
self.ultrasonic = UltrasonicSensor(port)
self.speedStep = 32767 // maxSpeed
self.steeringStep = 32767 // maxSteering
self.active = True
def drive(self, x, y):
# map y (-32768 ~ +32767) to speed (+maxSpeed ~ -maxSpeed):
speed = -y // self.speedStep
# map x (-32768 ~ +32767) to steering (-maxSteering ~ +maxSteering):
steering = x // self.steeringStep
self.driver.drive(speed, steering)
def target(self, x):
self.cannon.run(-x // 327)
def fire(self):
brick.sound.file('cannon.wav')
def inactive(self):
self.active = False
self.drive(0, 0)
brick.sound.beep()
def autoStopLoop(robot):
while robot.active:
if robot.ultrasonic.distance() < 200:
robot.drive(0, 0)
wait(100)
def main():
brick.sound.beep()
joystickEvent = detectJoystick(['Controller'])
if joystickEvent:
robot = Robot(Motor(Port.D), Motor(Port.A), Motor(Port.B), 55, 200)
t = threading.Thread(target=autoStopLoop, args=(robot,))
t.start()
def onButtonPressed(code):
if code == BUTTON_X:
robot.inactive()
return False
if code == BUTTON_A:
robot.fire()
return True
def onLeftJoyChanged(x, y):
robot.drive(x, y)
def onRightJoyChanged(x, y):
robot.target(x)
joystick = JoyStick(joystickEvent)
joystick.setButtonHandler(onButtonPressed)
joystick.setJoyLeftHandler(onLeftJoyChanged)
joystick.setJoyRightHandler(onRightJoyChanged)
joystick.startLoop()
else:
brick.sound.beep()
main()