def setLedMatrixContent(address, pixels):
buffer = [0]*len(pixels)
i = 0
for p in pixels:
buffer[i] = p & 0x00FF
buffer[i+1] = p >> 8
i = i + 2
logi.logiWrite(address, tuple(buffer))
def setLedMatrixContent(address, pixels):
buffer = [0] * len(pixels)
i = 0
for p in pixels:
buffer[i] = p & 0x00FF
buffer[i + 1] = p >> 8
i = i + 2
logi.logiWrite(address, tuple(buffer))
def setSegBCD(address, val): decode_sseg = [0x3F, 0x06, 0x5B, 0x4F, 0x66, 0x6D, 0x7D, 0x07, 0x7F, 0x6F, 0x77, 0x7C, 0x00] buf = () for i in val : buf = buf + (decode_sseg[i],) while len(buf) < 6 : buf = buf + (0x00,) logi.logiWrite(address, buf)
def setSegBCD(address, val):
decode_sseg = [
0x3F, 0x06, 0x5B, 0x4F, 0x66, 0x6D, 0x7D, 0x07, 0x7F, 0x6F, 0x77, 0x7C,
0x00
]
buf = ()
for i in val:
buf = buf + (decode_sseg[i], )
while len(buf) < 6:
buf = buf + (0x00, )
logi.logiWrite(address, buf)
def setServoFailSafeAngle(address,
index,
angle,
max_pulse=255.0,
min_angle=-45.0,
max_angle=45.0):
quanta = max_pulse / (max_angle - min_angle)
pulse = (max_pulse / 2) + (quanta * angle)
pulse = max(min(int(round(pulse)), 255), 0)
logi.logiWrite(address + ((index * 2) + 1),
((pulse & 0x00FF), ((pulse >> 8) & 0x00FF)))
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 setter(address, data): # fpga_write
"""
Set/Write 2 Bytes (16 bits)
@param address
@param data
@returns None
@brief Setter (Write)
"""
# TODO - ??? struct unpack more efficient ???
lsb = (data >> 0) & 0x00FF
msb = (data >> 8) & 0x00FF
logi.logiWrite(address, (lsb, msb))
def setter(address, data):
"""
Set/Write 2 Bytes (16 bits)
@param address
@param data
@returns None
@brief Setter (Write)
"""
# swap the bytes, logi.logiWrite is backwards...
# TODO - ??? struct unpack more efficient ???
lsb = (data >> 0) & 0x00FF
msb = (data >> 8) & 0x00FF
# logiWrite(0xE001, (0x00, 0xFF)) # LSB, MSB
logi.logiWrite(address, (lsb, msb))
def resetWatchdog(address):
logi.logiWrite(address, (0x01, 0x00))
def disableWatchdog(address):
logi.logiWrite(address, (0x00, 0x00))
import logi THRESHOLD = 0x7F logi.logiWrite(0x1004, (THRESHOLD, THRESHOLD))
for p in pixels:
buffer[i] = p & 0x00FF
buffer[i + 1] = p >> 8
i = i + 2
return tuple(buffer)
map = ()
count = 0
for c in range(1024):
r = count << 8
map = map + (r & 0xFF, ((r >> 8) & 0xFF))
count = count + 1
if count > 15:
count = 0
logi.logiWrite(MAT_ADDR, map)
time.sleep(1)
map = ()
g = 0x00F0
count = 0
for c in range(1024):
g = count << 4
map = map + (g & 0xFF, ((g >> 8) & 0xFF))
count = count + 1
if count > 15:
count = 0
logi.logiWrite(MAT_ADDR, map)
time.sleep(1)
def setServoFailSafePulse(address, index, pos): logi.logiWrite(address+((index * 2) + 1), ((pos & 0x00FF), ((pos >> 8) & 0x00FF)));
def disableWatchdog(address): logi.logiWrite(address, (0x00, 0x00) );
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)
def setGPIOVal(address, val): logi.logiWrite(address, ((val & 0x00FF), ((val >> 8) & 0x00FF)));
def setPWMPulse(address, chan, pulse): logi.logiWrite(((address+2)+chan), ((pulse & 0x00FF), ((pulse >> 8) & 0x00FF)));
def setPWMDivider(address, divider): logi.logiWrite(address, ((divider & 0x00FF), ((divider >> 8) & 0x00FF)));
def write_pixels(pixels):
frame_buffer = buffer_from_pixels(pixels)
if has_logi:
logi.logiWrite(MAT_ADDR, frame_buffer)
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"
return self.current_speed
class SimulatedSpeedService():
def __init__(self):
self.current_speed = 0.0 ;
def getSpeed(self):
return self.current_speed
if __name__ == "__main__":
speed_service = SpeedService()
logi.logiWrite(0x0000, (0x01, 0x01)) # enable watchdog
logi.logiWrite(0x0013, (128, 0x00)) # set failsafe value for speed
logi.logiWrite(0x0012, (128, 0x00)) # set current value for speed
P = 30.0
I = 0.75
D = -2.0
target_speed = 3.5
old_error = 0.0
integral = 0.0
while True:
logi.logiWrite(0x0000, (0x01, 0x01))
speed = speed_service.getSpeed()
error = (target_speed - speed)
derivative = error - old_error
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"
def setServoFailSafePulse(address, index, pos):
logi.logiWrite(address + ((index * 2) + 1),
((pos & 0x00FF), ((pos >> 8) & 0x00FF)))
def writeRegister(address, reg, val): read_val = logi.logiWrite(address+reg, ((val & 0x00FF), ((val >> 8) & 0x00FF)))
def setServoFailSafeAngle(address, index, angle, max_pulse=255.0, min_angle=-45.0, max_angle=45.0):
quanta = max_pulse/(max_angle-min_angle)
pulse = (max_pulse/2) + (quanta * angle)
pulse = max(min(int(round(pulse)), 255), 0)
logi.logiWrite(address+((index * 2) + 1), ((pulse & 0x00FF), ((pulse >> 8) & 0x00FF)));
def setPWMDivider(address, divider):
logi.logiWrite(address, ((divider & 0x00FF), ((divider >> 8) & 0x00FF)))
def setPWMPeriod(address, period): logi.logiWrite(address+1, ((period & 0x00FF), ((period >> 8) & 0x00FF)));
def setServoPulse(address, index, pos):
logi.logiWrite(address + (index * 2),
((pos & 0x00FF), ((pos >> 8) & 0x00FF)))
def setGPIODir(address, dir_mask): logi.logiWrite(address+1, ((dir_mask & 0x00FF), ((dir_mask >> 8) & 0x00FF)));
def setPWMPeriod(address, period):
logi.logiWrite(address + 1, ((period & 0x00FF), ((period >> 8) & 0x00FF)))
def setServoPulse(address, index, pos): logi.logiWrite(address+(index * 2), ((pos & 0x00FF), ((pos >> 8) & 0x00FF)));
def setPWMPulse(address, chan, pulse):
logi.logiWrite(((address + 2) + chan),
((pulse & 0x00FF), ((pulse >> 8) & 0x00FF)))
def enableWatchdog(address): logi.logiWrite(address, (0x01, 0x00) );
def setGPIODir(address, dir_mask):
logi.logiWrite(address + 1,
((dir_mask & 0x00FF), ((dir_mask >> 8) & 0x00FF)))
def resetWatchdog(address): logi.logiWrite(address, (0x01, 0x00) );
def setGPIOVal(address, val):
logi.logiWrite(address, ((val & 0x00FF), ((val >> 8) & 0x00FF)))
i = 0
for p in pixels:
buffer[i] = p & 0x00FF
buffer[i+1] = p >> 8
i = i + 2
return tuple(buffer)
map = ()
count = 0
for c in range(1024):
r = count << 8
map = map + (r & 0xFF, ((r >> 8) & 0xFF))
count = count + 1
if count > 15:
count = 0
logi.logiWrite(MAT_ADDR, map)
time.sleep(1)
map = ()
g = 0x00F0
count = 0
for c in range(1024):
g = count << 4
map = map + (g & 0xFF, ((g >> 8) & 0xFF))
count = count + 1
if count > 15:
count = 0
logi.logiWrite(MAT_ADDR, map)
time.sleep(1)
def writeRegister(address, reg, val):
read_val = logi.logiWrite(address + reg,
((val & 0x00FF), ((val >> 8) & 0x00FF)))
def enableWatchdog(address):
logi.logiWrite(address, (0x01, 0x00))
self.current_speed = DISTANCE_BETWEEN_TICKS / (float(enc_val) /
1000000.0)
return self.current_speed
class SimulatedSpeedService():
def __init__(self):
self.current_speed = 0.0
def getSpeed(self):
return self.current_speed
if __name__ == "__main__":
speed_service = SpeedService()
logi.logiWrite(0x0000, (0x01, 0x01)) # enable watchdog
logi.logiWrite(0x0013, (128, 0x00)) # set failsafe value for speed
logi.logiWrite(0x0012, (128, 0x00)) # set current value for speed
P = 30.0
I = 0.75
D = -2.0
target_speed = 3.5
old_error = 0.0
integral = 0.0
while True:
logi.logiWrite(0x0000, (0x01, 0x01))
speed = speed_service.getSpeed()
error = (target_speed - speed)
derivative = error - old_error
