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
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
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]))
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]))
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
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
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
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
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
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
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
def readRegister(address, reg): read_val = logi.logiRead(address+reg, 2); return read_val[0]+(read_val[1] << 8)
def getGPIOVal(address): read_val = logi.logiRead(address, 2); return read_val[0]+(read_val[1] << 8)
def readRegister(address, reg):
read_val = logi.logiRead(address + reg, 2)
return read_val[0] + (read_val[1] << 8)
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"
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"
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)
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"
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"
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
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
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
def getGPIOVal(address):
read_val = logi.logiRead(address, 2)
return read_val[0] + (read_val[1] << 8)