def absolute_line_pos(): # line_pos=[0]*5 # white_line=line_sensor.get_white_line() white_line = get_white_line() # print "White: " + str(white_line) black_line=get_black_line() # print "Black: " + str(black_line) range_sensor= get_range() # print "Range: " + str(range_sensor) threshold=[a+b/2 for a,b in zip(white_line,range_sensor)] # print "Threshold:" + str(threshold) raw_vals=line_sensor.get_sensorval() # print (raw_vals) # updated to handle the case where the line follower is not answering for i in range(5): if raw_vals[i] == -1: line_pos[i] = -1 elif raw_vals[i]>threshold[i]: line_pos[i]=1 else: line_pos[i]=0 # print line_pos return line_pos
def absolute_line_pos():
# line_pos=[0]*5
# white_line=line_sensor.get_white_line()
white_line = get_white_line()
# print "White: " + str(white_line)
black_line = get_black_line()
# print "Black: " + str(black_line)
range_sensor = get_range()
# print "Range: " + str(range_sensor)
threshold = [a + b / 2 for a, b in zip(white_line, range_sensor)]
# print "Threshold:" + str(threshold)
raw_vals = line_sensor.get_sensorval()
# print (raw_vals)
# updated to handle the case where the line follower is not answering
for i in range(5):
if raw_vals[i] == -1:
line_pos[i] = -1
elif raw_vals[i] > threshold[i]:
line_pos[i] = 1
else:
line_pos[i] = 0
# print line_pos
return line_pos
def absolute_line_pos(): raw_vals = line_sensor.get_sensorval() for i in range(5): if raw_vals[i] > threshold[i]: line_pos[i] = 1 else: line_pos[i] = 0 return line_pos
def absolute_line_pos():
raw_vals = line_sensor.get_sensorval()
for i in range(5):
if raw_vals[i] > threshold[i]:
line_pos[i] = 1
else:
line_pos[i] = 0
return line_pos
def absolute_line_pos(): # line_pos=[0]*5 # white_line=line_sensor.get_white_line() white_line = get_white_line() # print "White: " + str(white_line) black_line = get_black_line() # print "Black: " + str(black_line) range_sensor = get_range() # print "Range: " + str(range_sensor) threshold = [a + b / 2 for a, b in zip(white_line, range_sensor)] # print "Threshold:" + str(threshold) raw_vals = line_sensor.get_sensorval() for i in range(5): if raw_vals[i] > threshold[i]: line_pos[i] = 1 else: line_pos[i] = 0 return line_pos
def absolute_line_pos():
# line_pos=[0]*5
# white_line=line_sensor.get_white_line()
white_line = get_white_line()
# print "White: " + str(white_line)
black_line = get_black_line()
# print "Black: " + str(black_line)
range_sensor = get_range()
# print "Range: " + str(range_sensor)
threshold = [a + b / 2 for a, b in zip(white_line, range_sensor)]
# print "Threshold:" + str(threshold)
raw_vals = line_sensor.get_sensorval()
for i in range(5):
if raw_vals[i] > threshold[i]:
line_pos[i] = 1
else:
line_pos[i] = 0
return line_pos
# Dexter Industries line sensor basic example
#
# This example shows a basic example of how to read sensor data from the line sensor.
from __future__ import print_function
from __future__ import division
from builtins import input
import line_sensor
import time
line_pos = [0] * 5
white_line = line_sensor.get_white_line()
black_line = line_sensor.get_black_line()
range_sensor = line_sensor.get_range()
threshold = [a + b / 2 for a, b in zip(white_line, range_sensor)]
while True:
raw_vals = line_sensor.get_sensorval()
for i in range(5):
if raw_vals[i] > threshold[i]:
line_pos[i] = 1
else:
line_pos[i] = 0
print(line_pos)
def white_line_set_button_OnButtonClick(self,event):
for i in range(2):
line_sensor.get_sensorval()
line_sensor.set_white_line()
line_val=line_sensor.get_white_line()
self.label.SetLabel("White Line : "+str(line_val))
def black_line_set_OnButtonClick(self,event):
for i in range(2):
line_sensor.get_sensorval()
line_sensor.set_black_line()
line_val=line_sensor.get_black_line()
self.label.SetLabel("Black Line : "+str(line_val))
def white_line_set_button_OnButtonClick(self, event):
for i in range(2):
line_sensor.get_sensorval()
line_sensor.set_white_line()
line_val = line_sensor.get_white_line()
self.label.SetLabel("White Line : " + str(line_val))
def black_line_set_OnButtonClick(self, event):
for i in range(2):
line_sensor.get_sensorval()
line_sensor.set_black_line()
line_val = line_sensor.get_black_line()
self.label.SetLabel("Black Line : " + str(line_val))
GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/gpl-3.0.txt>. ''' import line_sensor import time line_pos=[0]*5 white_line=line_sensor.get_white_line() black_line=line_sensor.get_black_line() range_sensor= line_sensor.get_range() threshold=[a+b/2 for a,b in zip(white_line,range_sensor)] # print white_line # print black_line # print range # print threshold while True: raw_vals=line_sensor.get_sensorval() for i in range(5): if raw_vals[i]>threshold[i]: line_pos[i]=1 else: line_pos[i]=0 print line_pos
def run():
GPG = gopigo3.GoPiGo3()
server_socket = socket.socket()
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(('0.0.0.0', 8002))
server_socket.listen(0)
while True:
print "Matlab server awaiting connection"
GPG.reset_all()
GPG.set_grove_type(GPG.GROVE_1, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_1, GPG.GROVE_INPUT_ANALOG)
GPG.set_grove_type(GPG.GROVE_2, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_2, GPG.GROVE_INPUT_ANALOG)
connection = server_socket.accept()[0]
print "Matlab server connected"
had_command = True
last_battery_good = time.time()
try:
while True:
# Battery check
battery = GPG.get_voltage_battery()
if battery < 10:
print "Low battery: ", battery
if time.time() > last_battery_good + 10:
print "Battery critically low, shutting down"
os.system("sudo shutdown now -h")
else:
last_battery_good = time.time()
ready = select.select([connection], [], [],
1.0)[0] # 1000ms timeout
if ready:
had_command = True
sz = struct.unpack('<L',
connection.recv(
struct.calcsize('<L')))[0]
if sz:
msg = connection.recv(sz)
msg = struct.unpack('<lll', msg)
# Apply commands
if battery > 10.5:
GPG.set_motor_dps(GPG.MOTOR_LEFT, msg[0])
GPG.set_motor_dps(GPG.MOTOR_RIGHT, msg[1])
GPG.set_servo(GPG.SERVO_1, msg[2])
else:
print "Not executing command due to low battery (", battery, "V)"
# Construct status
msg = struct.pack(
'<ll',
GPG.get_motor_encoder(GPG.MOTOR_LEFT) *
GPG.MOTOR_TICKS_PER_DEGREE,
GPG.get_motor_encoder(GPG.MOTOR_RIGHT) *
GPG.MOTOR_TICKS_PER_DEGREE)
msg = msg + struct.pack('<lllll',
*line_sensor.get_sensorval())
msg = msg + struct.pack('<f', battery)
msg = msg + struct.pack(
'<llll', grovepi.analogRead(0), grovepi.analogRead(1),
grovepi.analogRead(2), grovepi.analogRead(3))
msg = msg + struct.pack(
'<ll', GPG.get_grove_analog(GPG.GROVE_1_1),
GPG.get_grove_analog(GPG.GROVE_2_1))
msg = struct.pack('<L', len(msg)) + msg
connection.send(msg)
else:
if had_command:
print "Command timeout"
GPG.set_motor_dps(GPG.MOTOR_LEFT, 0)
GPG.set_motor_dps(GPG.MOTOR_RIGHT, 0)
had_command = False
except:
print "Matlab server error"
pass
finally:
try:
connection.close()
except:
pass
server_socket.close()
def run():
GPG = gopigo3.GoPiGo3()
last_command = time.time()
last_battery_good = time.time()
server_socket = socket.socket()
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(('0.0.0.0', 8002))
server_socket.listen(0)
while True:
print "Data server awaiting connection"
GPG.reset_all()
GPG.set_grove_type(GPG.GROVE_1, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_1, GPG.GROVE_INPUT_ANALOG)
GPG.set_grove_type(GPG.GROVE_2, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_2, GPG.GROVE_INPUT_ANALOG)
GPG.set_led(GPG.LED_WIFI, 0, 0, 0)
try:
grovepi.analogRead(0)
has_grove = True
except:
has_grove = False
print "Grove board detected: ", has_grove
connection = server_socket.accept()[0]
print "Data server connected"
try:
while True:
# Battery check
battery = GPG.get_voltage_battery()
if battery < 10:
print "Low battery: ", battery
if time.time() > last_battery_good + 1:
print "Battery critically low, shutting down"
os.system("shutdown now -h")
else:
last_battery_good = time.time()
ready = select.select([connection], [], [],
0.100)[0] # 100ms timeout
if ready:
sz = struct.unpack('<L',
connection.recv(
struct.calcsize('<L')))[0]
if not sz:
break
msg = ''
while len(msg) < sz:
msg = connection.recv(sz - len(msg))
# Apply commands
if sz == 12:
msg = struct.unpack('<lll', msg)
msg = msg + (0, 0, 0)
elif sz == 15:
msg = struct.unpack('<lllBBB', msg)
else:
print "Unknown message size ", sz
break
if battery > 10.5:
GPG.set_motor_dps(GPG.MOTOR_LEFT, msg[0])
GPG.set_motor_dps(GPG.MOTOR_RIGHT, msg[1])
GPG.set_servo(GPG.SERVO_1, msg[2])
GPG.set_servo(GPG.SERVO_2, msg[2])
GPG.set_led(GPG.LED_WIFI, msg[3], msg[4], msg[5])
last_command = time.time()
# Auto-stop if no commands are sent
if time.time() > last_command + 1:
print "Stopping (no data or low battery). Battery is at ", battery, "V"
last_command = time.time() + 3600
GPG.reset_all()
# Construct status
msg = struct.pack(
'<ll',
GPG.get_motor_encoder(GPG.MOTOR_LEFT) *
GPG.MOTOR_TICKS_PER_DEGREE,
GPG.get_motor_encoder(GPG.MOTOR_RIGHT) *
GPG.MOTOR_TICKS_PER_DEGREE)
msg = msg + struct.pack('<lllll', *line_sensor.get_sensorval())
msg = msg + struct.pack('<f', battery)
if has_grove:
msg = msg + struct.pack(
'<llll', grovepi.analogRead(1), grovepi.analogRead(0),
grovepi.analogRead(2), grovepi.analogRead(3))
msg = msg + struct.pack(
'<ll', GPG.get_grove_analog(GPG.GROVE_1_1),
GPG.get_grove_analog(GPG.GROVE_2_1))
msg = struct.pack('<L', len(msg)) + msg
connection.send(msg)
except Exception as e:
print "Data server error: ", e
pass
finally:
try:
connection.close()
except:
pass
server_socket.close()
