forked from DexterInd/BrickPi_Scratch
/
BrickPiScratch_EV3.py
229 lines (208 loc) · 8.3 KB
/
BrickPiScratch_EV3.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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
# Jaikrishna
# Initial Date: June 26, 2013
#
# Exception handling and recovery code added
# Karan Nayan
# Last Updated: June 3, 2014
#
# This file is for interfacing Scratch with BrickPi
# The Python program acts as the Bridge between Scratch & BrickPi and must be running for the Scratch program to run.
# Requirements :
# Prior to running this progam, ScratchPy must be installed on the system. Refer BrickPi documentation on how to install ScratchPy.
# The BrickPi python library file (BrickPi.py) must be placed in the same path as this file.
# Remote Sensor values must be enabled in Scratch
# This python code must be restarted everytime you need to run a new program.
# To run this in python, Open up Terminal and navigate to the path using 'cd' command
# Then enter:
# python BrickPiScratch.py
# To exit python, press Ctrl+C or force close the Terminal
# Broadcasts from Python:
# 'READY' tells that BrickPi serial setup succeeded. Use 'When I receive READY' to specify starting point of program.
# 'UPDATED' tells that sensor values of Scratch has been updated from BrickPi
# Broadcast from Scratch:
# 'SETUP' command sets up the sensor properties
# 'START' command tells RPi to start continuous transmission to BPi
# 'UPDATE' command calls for an updation of Sensor Values of Scratch
# 'STOP' command stops the continuous up
# SETUP and START must be done only once after configuring the Sensors. UPDATE is Required atleast once.
# Setting Sensor type:
# S1 ULTRASONIC
# S2 TOUCH
# S3 RAW
# S4 COLOR
# S3 FLEX
# S5 TEMP
# Note: Only these sensors are supported as of now. The first 4 are lego products while the last 2 are from DexterIndustries
# Enabling and Running Motors:
# MA E - Enable motor A
# MB D - Disable motor B
# MA 100 - Set MotorA speed to 100
# MB -50 - Set MotorB speed to -100
import scratch,sys,threading,math
from BrickPi import *
try:
s = scratch.Scratch()
if s.connected:
print "BrickPi Scratch: Connected to Scratch successfully"
#else:
#sys.exit(0)
except scratch.ScratchError:
print "BrickPi Scratch: Scratch is either not opened or remote sensor connections aren't enabled"
#sys.exit(0)
# The sensor types below need to be different for EV3 sensors
# See the Python sensor examples for reference
sensor = [ None, False , False , False , False ]
spec = [ None, 0 , 0 , 0 , 0 ]
stype = { 'ULTRASONIC' : TYPE_SENSOR_EV3_US_M0,
'TOUCH' : TYPE_SENSOR_EV3_TOUCH_0,
'COLOR' : TYPE_SENSOR_EV3_COLOR_M2,
'RAW' : TYPE_SENSOR_RAW,
'TEMP' : TYPE_SENSOR_RAW,
'FLEX' : TYPE_SENSOR_RAW}
BrickPiSetup()
def comp(val , case):
if val == None or val== 0:
return 0
if case == 1:
return val-600
elif case == 2 :
_a = [0.003357042, 0.003354017, 0.0033530481, 0.0033536166]
_b = [0.00025214848, 0.00025617244, 0.00025420230, 0.000253772]
_c = [0.0000033743283, 0.0000021400943, 0.0000011431163, 0.00000085433271]
_d = [-0.000000064957311, -0.000000072405219, -0.000000069383563, -0.000000087912262]
RtRt25 = (float)(val) / (1023 - val)
lnRtRt25 = math.log(RtRt25)
if (RtRt25 > 3.277) :
i = 0
elif (RtRt25 > 0.3599) :
i = 1
elif (RtRt25 > 0.06816) :
i = 2
else :
i = 3
temp = 1.0 / (_a[i] + (_b[i] * lnRtRt25) + (_c[i] * lnRtRt25 * lnRtRt25) + (_d[i] * lnRtRt25 * lnRtRt25 * lnRtRt25))
temp-=273
return round(temp,2)
running = False
class myThread (threading.Thread): #This thread is used for continuous transmission to BPi while main thread takes care of Rx/Tx Broadcasts of Scratch
def __init__(self, threadID, name, counter):
threading.Thread.__init__(self)
self.threadID = threadID
self.name = name
self.counter = counter
def run(self):
while running:
BrickPiUpdateValues() # Ask BrickPi to update values for sensors/motors
time.sleep(.2) # sleep for 200 ms
thread1 = myThread(1, "Thread-1", 1) #Setup and start the thread
thread1.setDaemon(True)
try:
s.broadcast('READY')
except NameError:
print "BrickPi Scratch: Unable to Broadcast"
while True:
try:
m = s.receive()
while m[0] == 'sensor-update' :
m = s.receive()
msg = m[1]
if msg == 'SETUP' :
BrickPiSetupSensors()
print "BrickPi Scratch: Setting up sensors done"
elif msg == 'START' :
running = True
if thread1.is_alive() == False:
thread1.start()
print "BrickPi Scratch: Service Started"
elif msg == 'STOP' :
running = False
elif msg == 'UPDATE' :
if sensor[1] :
if spec[1] :
s.sensorupdate({'S1' : comp(BrickPi.Sensor[PORT_1],spec[1])})
else:
s.sensorupdate({'S1' : BrickPi.Sensor[PORT_1]})
if sensor[2] :
if spec[2] :
s.sensorupdate({'S2' : comp(BrickPi.Sensor[PORT_2],spec[2])})
else :
s.sensorupdate({'S2' : BrickPi.Sensor[PORT_2]})
if sensor[3] :
if spec[3] :
s.sensorupdate({'S3' : comp(BrickPi.Sensor[PORT_3],spec[3])})
else :
s.sensorupdate({'S3' : BrickPi.Sensor[PORT_3]})
if sensor[4] :
if spec[4] :
s.sensorupdate({'S4' : comp(BrickPi.Sensor[PORT_4],spec[4])})
else:
s.sensorupdate({'S4' : BrickPi.Sensor[PORT_4]})
s.broadcast('UPDATED')
elif msg[:2] == 'S1' :
if msg[2:].strip() == 'FLEX' :
spec[1] = 1
elif msg[2:].strip() == 'TEMP' :
spec[1] = 2
BrickPi.SensorType[PORT_1] = stype[msg[2:].strip()]
sensor[1] = True
elif msg[:2] == 'S2' :
if msg[2:].strip() == 'FLEX' :
spec[2] = 1
elif msg[2:].strip() == 'TEMP' :
spec[2] = 2
BrickPi.SensorType[PORT_2] = stype[msg[2:].strip()]
sensor[2] = True
elif msg[:2] == 'S3' :
if msg[2:].strip() == 'FLEX' :
spec[3] = 1
elif msg[2:].strip() == 'TEMP' :
spec[3] = 2
BrickPi.SensorType[PORT_3] = stype[msg[2:].strip()]
sensor[3] = True
elif msg[:2] == 'S4' :
if msg[2:].strip() == 'FLEX' :
spec[4] = 1
elif msg[2:].strip() == 'TEMP' :
spec[4] = 2
BrickPi.SensorType[PORT_4] = stype[msg[2:].strip()]
sensor[4] = True
elif msg == 'MA E' or msg == 'MAE' :
BrickPi.MotorEnable[PORT_A] = 1
elif msg == 'MB E' or msg == 'MBE' :
BrickPi.MotorEnable[PORT_B] = 1
elif msg == 'MC E' or msg == 'MCE' :
BrickPi.MotorEnable[PORT_C] = 1
elif msg == 'MD E' or msg == 'MDE' :
BrickPi.MotorEnable[PORT_D] = 1
elif msg == 'MA D' or msg == 'MAD' :
BrickPi.MotorEnable[PORT_A] = 0
elif msg == 'MB D' or msg == 'MBD' :
BrickPi.MotorEnable[PORT_B] = 0
elif msg == 'MC D' or msg == 'MCD' :
BrickPi.MotorEnable[PORT_C] = 0
elif msg == 'MD D' or msg == 'MDD' :
BrickPi.MotorEnable[PORT_D] = 0
elif msg[:2] == 'MA' :
BrickPi.MotorSpeed[PORT_A] = int(msg[2:])
elif msg[:2] == 'MB' :
BrickPi.MotorSpeed[PORT_B] = int(msg[2:])
elif msg[:2] == 'MC' :
BrickPi.MotorSpeed[PORT_C] = int(msg[2:])
elif msg[:2] == 'MD' :
BrickPi.MotorSpeed[PORT_D] = int(msg[2:])
except KeyboardInterrupt:
running= False
print "BrickPi Scratch: Disconnected from Scratch"
break
except (scratch.scratch.ScratchConnectionError,NameError) as e:
while True:
#thread1.join(0)
print "BrickPi Scratch: Scratch connection error, Retrying"
time.sleep(5)
try:
s = scratch.Scratch()
s.broadcast('READY')
print "BrickPi Scratch: Connected to Scratch successfully"
break;
except scratch.ScratchError:
print "BrickPi Scratch: Scratch is either not opened or remote sensor connections aren't enabled\n..............................\n"