-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathbluetooth_comms.py
112 lines (100 loc) · 2.71 KB
/
bluetooth_comms.py
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
104
105
106
107
108
109
110
111
112
#!/usr/bin/env python3
# Import ev3dev2
from ev3dev2.button import *
from ev3dev2.console import *
from ev3dev2.motor import *
from ev3dev2.sensor import *
from ev3dev2.sensor.lego import *
from ev3dev2.led import *
from ev3dev2.sound import *
from ev3dev2.power import *
print("ev3dev2 Imported")
# Other Imports
from traceback import print_exc
from utils import *
from threading import Thread
from time import sleep
print("utils Imported")
# Initialize Motors
topLeft = LargeMotor(OUTPUT_C)
topRight = LargeMotor(OUTPUT_A)
bottomLeft = LargeMotor(OUTPUT_D)
bottomRight = LargeMotor(OUTPUT_B)
# Initialize Sensors
iF = Sensor(INPUT_1, driver_name = "ht-nxt-ir-seek-v2")
iB = Sensor(INPUT_2, driver_name = "ht-nxt-ir-seek-v2")
compass = Sensor(INPUT_3, driver_name = "ht-nxt-compass")
ultrasonic = UltrasonicSensor(INPUT_4)
# Set Sensor Modes
iF.mode = "AC-ALL"
iB.mode = "AC-ALL"
compass.mode = "COMPASS"
ultrasonic.mode = "US-DIST-CM"
# Initialize Brick Functions
buttons = Button()
sound = Sound()
leds = Leds()
battery = PowerSupply()
paused = False
# Variables
fieldWidth=(1700)/2 # Min [650-700], Max [1000]
topspeed=90
speed=60
slowspeed=30
sp=speed
goal=compass.value()
ultrasonic_values=[]
outliers=0
# COMM STATES:
# -1 : Offline
# 0 : Idle
# 1 : Attacking
# 2 : Defending
from recode import comms
# Coast Motors
def coast():
topRight.off(brake=False)
bottomRight.off(brake=False)
topLeft.off(brake=False)
bottomLeft.off(brake=False)
if comms.Begin(comms,0) != 0: # Begin Bluetooth Comms
comms.state = 0
comms_thread = threading.Thread(target=comms.loop)
comms_thread.start()
leds.set_color('LEFT', 'AMBER')
leds.set_color('RIGHT', 'AMBER')
sound.set_volume(20)
sound.play_tone(650,0.3,0,20,sound.PLAY_NO_WAIT_FOR_COMPLETE)
print("Battery:",round(battery.measured_volts,2))
try:
while not buttons.right: sleep(0.05)
while buttons.right: sleep(0.05)
leds.set_color('LEFT', 'GREEN')
leds.set_color('RIGHT', 'GREEN')
while True:
if buttons.right:
coast()
paused = not paused
sound.play_tone(450,0.3,0,20,sound.PLAY_NO_WAIT_FOR_COMPLETE) if paused else sound.play_tone(650,0.3,0,20,sound.PLAY_NO_WAIT_FOR_COMPLETE)
while buttons.right:
sleep(0.05)
if buttons.backspace or buttons.left:
break
if paused: continue
print(comms.state + " | " + comms.teammate + " ",end='\r')
comms.state+=1
except Exception:
print("Error")
print_exc()
comms.enabled = False
coast() # Stop Motors
sleep(1)
except:
print("Interrupted")
comms.enabled = False
coast() # Stop Motors
sleep(1)
print("Ended")
comms.enabled = False
coast() # Stop Motors
sleep(1)