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