Esempio n. 1
0
def calibrate_line():
	for i in range(10):
		pos = logi.logiRead(0x2000, 320*2)
		for j in range(320):
			posy = pos[j*2] + (pos[j*2+1] << 8)
			line_pos[j] = line_pos[j] + posy
		time.sleep(0.1)

	for j in range(320):
                line_pos[j] = line_pos[j]/10
	def getSpeed(self):
		enc_reg = logi.logiRead(ENCODER_PERIOD_ADDRESS, 2)
        	enc_val = (enc_reg[1] << 8) | enc_reg[0]
        	if enc_val == 32767: # cannot measure tick period smaller than 32767us
                	self.current_speed = 0.0
		elif enc_val == 0: # saturate to maximum known speed ... should never reach that
			self.current_speed = 15.0
		else:
        		self.current_speed = DISTANCE_BETWEEN_TICKS/(float(enc_val)/1000000.0)
		return self.current_speed
Esempio n. 3
0
 def getSpeed(self):
     enc_reg = logi.logiRead(ENCODER_PERIOD_ADDRESS, 2)
     enc_val = (enc_reg[1] << 8) | enc_reg[0]
     if enc_val == 32767:  # cannot measure tick period smaller than 32767us
         self.current_speed = 0.0
     elif enc_val == 0:  # saturate to maximum known speed ... should never reach that
         self.current_speed = 15.0
     else:
         self.current_speed = DISTANCE_BETWEEN_TICKS / (float(enc_val) /
                                                        1000000.0)
     return self.current_speed
Esempio n. 4
0
def switch_laser(s=1, on=1):

    logi.logiWrite(0x1003, (0x01, 0x00))
    pin = logi.logiRead(0x1002, 2)
    if s == 1:
        lower_byte = pin[0] ^ 0x01
    else:
        if on == 0:
            lower_byte = pin[0] | 0x01
        else:
            lower_byte = pin[0] & (~0x01)
    logi.logiWrite(0x1002, (lower_byte, pin[1]))
Esempio n. 5
0
def switch_laser(s = 1, on = 1):
	
	logi.logiWrite(0x1003, (0x01, 0x00))
	pin = logi.logiRead(0x1002, 2)
	if s == 1:
		lower_byte = pin[0] ^ 0x01
	else :
		if on == 0:
			lower_byte = pin[0] | 0x01
		else:
			lower_byte = pin[0] & (~0x01)
	logi.logiWrite(0x1002, (lower_byte, pin[1]))
Esempio n. 6
0
def getter(address):
    """
    Get/Read 2 Bytes (16 bits)
    @param address
    @returns data
    @brief Getter (Read)
    """

    lsb, msb = logi.logiRead(address, 2) # tuple of 2 bytes

    # convert the two bytes to a single 16-bit integer value
    upper_byte = (msb << 8) & 0xFF00
    lower_byte = (lsb << 0) & 0x00FF

    return upper_byte + lower_byte
Esempio n. 7
0
    def getPosition(self):
        frame = logi.logiRead(0x080, 82)
        frame_size = frame[0]
        nmea_str = "".join(str(unichr(a)) for a in frame[2:frame_size + 2])
        nmea_fields = split(nmea_str, ',')
        if nmea_fields[6] > '0':
            lat = float(nmea_fields[2])
            long = float(nmea_fields[4])
            time = float(nmea_fields[1])
            if nmea_fields[3] == "S":
                lat = -lat
            if nmea_fields[5] == "W":
                long = -long
            self.current_pos = GpsPoint(DmToD(lat), DmToD(long), time, True)
            self.current_pos.setDilution(float(nmea_fields[8]) * 7.0)

        return self.current_pos
Esempio n. 8
0
	def getPosition(self):
		frame = logi.logiRead(0x080, 82)
                frame_size = frame[0]
                nmea_str = "".join(str(unichr(a)) for a in frame[2:frame_size+2])
		nmea_fields =  split(nmea_str, ',')
		if nmea_fields[6] > '0':
			lat = float(nmea_fields[2])
			long = float(nmea_fields[4])
			time = float(nmea_fields[1])
			if nmea_fields[3] == "S":
				lat = -lat
			if nmea_fields[5] == "W":
				long = -long
			self.current_pos = GpsPoint(DmToD(lat), DmToD(long), time, True)
			self.current_pos.setDilution(float(nmea_fields[8])*7.0)
			
		return self.current_pos
Esempio n. 9
0
def detect_objects():
	pos = logi.logiRead(0x2000, 320*2)
	objects = []
	no_obj = True
	last_object_id = 0
	for i in range(320):
		posy = pos[i*2] + (pos[i*2+1] << 8)
		posy = posy - line_pos[i]
		if abs(posy) > 15:
			if no_obj :
				objects.append([i, posy])
				no_obj = False
			else:
				if i <= objects[last_object_id][0]+objects[last_object_id][1]:
					objects[last_object_id][1] = objects[last_object_id][1]+1
				else:
					objects.append([i, 1])
					last_object_id = last_object_id + 1
	#print objects
	obj_list = []
	for obj in objects:
		if obj[1] > 10:
			obj_list.append([obj[0]+(obj[1]/2), obj[1]])
	return obj_list
Esempio n. 10
0
def detect_objects():
    pos = logi.logiRead(0x2000, 320 * 2)
    objects = []
    no_obj = True
    last_object_id = 0
    for i in range(320):
        posy = pos[i * 2] + (pos[i * 2 + 1] << 8)
        posy = posy - line_pos[i]
        if abs(posy) > 15:
            if no_obj:
                objects.append([i, posy])
                no_obj = False
            else:
                if i <= objects[last_object_id][0] + objects[last_object_id][1]:
                    objects[last_object_id][1] = objects[last_object_id][1] + 1
                else:
                    objects.append([i, 1])
                    last_object_id = last_object_id + 1
    #print objects
    obj_list = []
    for obj in objects:
        if obj[1] > 10:
            obj_list.append([obj[0] + (obj[1] / 2), obj[1]])
    return obj_list
Esempio n. 11
0
import logi
import time

a = logi.logiRead(0x0200, 6)
available = a[2] + (a[3] << 8) + (a[4] << 16) + (a[5] << 24)
print available

a = logi.logiRead(0x1001, 2)
flushed = a[0] + (a[1] << 8)
print "flushed : " + str(flushed)

a = logi.logiRead(0x1000, 2)
refreshed = a[0] + (a[1] << 8)
print "refreshed : " + str(refreshed)

a = logi.logiRead(0x1002, 2)
refreshed = a[0] + (a[1] << 8)
print "refresh address : " + str(refreshed)


no_error = True
old_val = -1
dist_between_errors = 0
last_error = 0
count = 0
# time.sleep(2)
while no_error and available > 0:
    data = logi.logiRead(0x0000, 1024)
    for j in range(512):
        val = data[j * 2] + (data[(j * 2) + 1] << 8)
        # print val
Esempio n. 12
0
import logi
import time

a = logi.logiRead(0x0200, 6)
available = a[2] + (a[3] << 8) + (a[4] << 16) + (a[5] << 24)
print available

a = logi.logiRead(0x1001, 2)
flushed = a[0] + (a[1] << 8)
print "flushed : " + str(flushed)

a = logi.logiRead(0x1000, 2)
refreshed = a[0] + (a[1] << 8)
print "refreshed : " + str(refreshed)

a = logi.logiRead(0x1002, 2)
refreshed = a[0] + (a[1] << 8)
print "refresh address : " + str(refreshed)

no_error = True
old_val = -1
dist_between_errors = 0
last_error = 0
count = 0
#time.sleep(2)
while no_error and available > 0:
    data = logi.logiRead(0x0000, 1024)
    for j in range(512):
        val = data[j * 2] + (data[(j * 2) + 1] << 8)
        #print val
        count = count + 1
	nes2_b = 0
	nes2_up = 0
	nes2_down = 0
	nes2_left = 0
	nes2_right = 0
	nes2_select = 0
	nes2_start = 0

	i = MIN_ANGLE;
	mode = 0  # going up
	servo_angle1 = 0
	servo_angle2 = 0
	while True:
	
		#min_max()
		nes_val = logi.logiRead(0x20,2)
		#print "nes1_val:	", nes_val[0]
		nes1_a = 		nes_val[0]&0x0001
		nes1_b = 		(nes_val[0]&0x0002)>>1
		nes1_up = 		(nes_val[0]&0x0004)>>2
		nes1_down = 	(nes_val[0]&0x0008)>>3
		nes1_left = 	(nes_val[0]&0x0010)>>4
		nes1_right = 	(nes_val[0]&0x0020)>>5
		nes1_select = 	(nes_val[0]&0x0040)>>6
		nes1_start = 	(nes_val[0]&0x0080)>>7
		# print "nes1_a:		", nes1_a
		# print "nes1_b:		", nes1_b
		#print "nes1_up:	", nes1_up
		#print "nes1_down:	", nes1_down
		# print "nes1_left:	", nes1_left
		# print "nes1_right:	", nes1_right
Esempio n. 14
0
def readRegister(address, reg):	
		read_val = logi.logiRead(address+reg, 2);
		return read_val[0]+(read_val[1] << 8)
Esempio n. 15
0
def getGPIOVal(address):	
		read_val = logi.logiRead(address, 2);
		return read_val[0]+(read_val[1] << 8)
Esempio n. 16
0
def readRegister(address, reg):
    read_val = logi.logiRead(address + reg, 2)
    return read_val[0] + (read_val[1] << 8)
Esempio n. 17
0
import logi
import time
from binascii import *
from string import *

while True:
	time.sleep(0.1)
	logi.logiWrite(0x0004, (0xFF, 0xFF))
	for i in range(3):
		sonar = logi.logiRead(0x0004+i, 2)
		sonar_tp = (sonar[1] << 8) | sonar[0]
		sonar_cm = float(sonar_tp)/170.0
		print "sonar "+str(i)+" :"+str(sonar_cm)+" cm"
Esempio n. 18
0
import logi
import time
from binascii import *
from string import *

while True:
    time.sleep(0.1)
    logi.logiWrite(0x0004, (0xFF, 0xFF))
    for i in range(3):
        sonar = logi.logiRead(0x0010 + i, 2)
        sonar_tp = (sonar[1] << 8) | sonar[0]
        sonar_cm = float(sonar_tp) / 59.0
        print "sonar " + str(i) + " :" + str(sonar_cm) + " cm"
Esempio n. 19
0
import logi
import time
import math
from binascii import *
from string import *

logi.logiWrite(0x000D, (0x01, 0x01))
logi.logiWrite(0x000D, (0x00, 0x00))


#calibration procedure:
#1) put the car on the floor and lay a meter or ruler on the side
#2) start the script and push the on a given distance (20cm to 1m)
#3) write-down the tick value at the end of the distance
#4) report the distance in the CALIBRATE_DISTANCE variable, and report the tick count in the CALIBRATE_TICK in the script
#5) run the script again, and verify that the reported distance is fine, you can adjust the CALIBRATE_TICK and CALIBRATE_DISTANCE to give 

CALIBRATE_TICK = 1.0
CALIBRATE_DISTANCE = 1.0 
CONV_FACTOR = CALIBRATE_DISTANCE/CALIBRATE_TICK

while True:
	enc_reg = logi.logiRead(0x000D, 2)
	enc_val = (enc_reg[1] << 8) | enc_reg[0]
	dist = float(enc_val)*CONV_FACTOR
	print "tick : "+str(enc_val)+"tick"
	print "dist : "+str(dist)+"m"
	time.sleep(0.1)
Esempio n. 20
0
import logi
import time
from binascii import *
from string import *



while True:
	time.sleep(0.1)
	logi.logiWrite(0x0004, (0xFF, 0xFF))
	for i in range(3):
		sonar = logi.logiRead(0x0010+i, 2)
		sonar_tp = (sonar[1] << 8) | sonar[0]
		sonar_cm = float(sonar_tp)/59.0
		print "sonar "+str(i)+" :"+str(sonar_cm)+" cm"
Esempio n. 21
0
import logi
import time
from binascii import *
from string import *

while True:
    time.sleep(0.1)
    logi.logiWrite(0x0004, (0xFF, 0xFF))
    for i in range(3):
        sonar = logi.logiRead(0x0004 + i, 2)
        sonar_tp = (sonar[1] << 8) | sonar[0]
        sonar_cm = float(sonar_tp) / 170.0
        print "sonar " + str(i) + " :" + str(sonar_cm) + " cm"
Esempio n. 22
0
    nes2_b = 0
    nes2_up = 0
    nes2_down = 0
    nes2_left = 0
    nes2_right = 0
    nes2_select = 0
    nes2_start = 0

    i = MIN_ANGLE
    mode = 0  # going up
    servo_angle1 = 0
    servo_angle2 = 0
    while True:

        #min_max()
        nes_val = logi.logiRead(0x20, 2)
        #print "nes1_val:	", nes_val[0]
        nes1_a = nes_val[0] & 0x0001
        nes1_b = (nes_val[0] & 0x0002) >> 1
        nes1_up = (nes_val[0] & 0x0004) >> 2
        nes1_down = (nes_val[0] & 0x0008) >> 3
        nes1_left = (nes_val[0] & 0x0010) >> 4
        nes1_right = (nes_val[0] & 0x0020) >> 5
        nes1_select = (nes_val[0] & 0x0040) >> 6
        nes1_start = (nes_val[0] & 0x0080) >> 7
        # print "nes1_a:		", nes1_a
        # print "nes1_b:		", nes1_b
        #print "nes1_up:	", nes1_up
        #print "nes1_down:	", nes1_down
        # print "nes1_left:	", nes1_left
        # print "nes1_right:	", nes1_right
Esempio n. 23
0
import logi
import time
from binascii import *
from string import *

# reset fifo
# logi.logiWrite(0x0010, (0x01, 0x01))
while True:
    time.sleep(0.1)
    frame = logi.logiRead(0x0080, 82)
    # gyro_tuple = logi.logiRead(0x0002, 2)
    # gyro_x = (gyro_tuple[1] << 8) | gyro_tuple[0]
    # if (gyro_tuple[1] & 0x80) != 0:
    # 		gyro_x = -32768 + (gyro_x & 0x7FFF)
    # 	print gyro_x

    # 	sonar = logi.logiRead(0x0041, 2)
    # 	sonar_tp = (sonar[1] << 8) | sonar[0]
    # 	sonar_cm = float(sonar_tp)/57.0
    # 	if sonar_cm > 4.0 :
    # 		print "sonar : "+str(sonar_cm)+" cm"
    frame_size = frame[0]
    # print frame
    # print frame_size
    # continue
    nmea_str = "".join(str(unichr(a)) for a in frame[2 : frame_size + 2])
    print nmea_str
    nmea_fields = split(nmea_str, ",")
    if nmea_fields[6] > "0":
        lat = float(nmea_fields[2]) / 100
        long = float(nmea_fields[4]) / 100
Esempio n. 24
0
import logi
import time
from binascii import *
from string import *

# reset fifo
#logi.logiWrite(0x0010, (0x01, 0x01))
while True:
	time.sleep(0.1)
	frame = logi.logiRead(0x0080, 82)
	#gyro_tuple = logi.logiRead(0x0002, 2)
	#gyro_x = (gyro_tuple[1] << 8) | gyro_tuple[0]
	#if (gyro_tuple[1] & 0x80) != 0:
#		gyro_x = -32768 + (gyro_x & 0x7FFF)
#	print gyro_x
	
#	sonar = logi.logiRead(0x0041, 2)
#	sonar_tp = (sonar[1] << 8) | sonar[0]
#	sonar_cm = float(sonar_tp)/57.0
#	if sonar_cm > 4.0 :
#		print "sonar : "+str(sonar_cm)+" cm"
	frame_size = frame[0]
	#print frame
	#print frame_size
	#continue
	nmea_str = "".join(str(unichr(a)) for a in frame[2:frame_size+2])
	print nmea_str
	nmea_fields =  split(nmea_str, ',')
	if nmea_fields[6] > '0':
		lat = float(nmea_fields[2])/100
		long = float(nmea_fields[4])/100
Esempio n. 25
0
def getGPIOVal(address):
    read_val = logi.logiRead(address, 2)
    return read_val[0] + (read_val[1] << 8)