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(): # 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()