class SONIC():
def __init__(self,uart_port,uart_baud):
self.sonic = UART(uart_port,uart_baud)
def clear(self):
if self.sonic.any():
data = self.sonic.read(100)
def read(self):
if self.sonic.any():
data = self.sonic.read(10)
try:
data = data.decode()
except UnicodeError:
return -1
data = data[0:len(data)-1]
return data
else:
return -1
def get_data(self):
self.clear()
time.sleep(2)
sonic_data = -1
while(sonic_data == -1):
sonic_data = self.read()
return sonic_data
class Sensor():
def __init__(self, num, value):
self.data1 = 0
self.data2 = 0
self.data3 = 0
self.data4 = 0
self.state = 0
self.uart = UART(num, value)
time.sleep(1)
def open_s(self):
data = [0x11, 0x02, 0x0b, 0x00, 0xe2]
for i in range(0, len(data)):
self.uart.writechar(data[i])
time.sleep(0.03)
while self.uart.any() == 0:
continue
self.uart.read()
def close_s(self):
data = [0x11, 0x03, 0x0c, 0x01, 0x1e, 0xc1]
for i in range(0, len(data)):
self.uart.writechar(data[i])
time.sleep(0.03)
while self.uart.any() == 0:
continue
self.uart.read()
def work_s(self):
data = [0x11, 0x01, 0x0b, 0xe3]
for i in range(0, len(data)):
self.uart.writechar(data[i])
time.sleep(0.03)
while self.uart.any() == 0:
continue
data_ = []
for i in range(0, 20):
a = self.uart.readchar()
if i >= 2:
data_.append(a)
self.data1 = data_[1] * 256**3 + data_[2] * 256**2 + data_[
3] * 256 + data_[4]
self.data2 = data_[5] * 256**3 + data_[6] * 256**2 + data_[
7] * 256 + data_[8]
self.data3 = data_[9] * 256 + data_[10]
self.data4 = data_[11] * 256 + data_[12]
def read_s(self):
if self.state == 3:
self.open_s()
self.state = 2
if self.state == 2:
self.work_s()
if self.state == 1:
self.close_s()
self.state = 0
return self.data1, self.data2, self.data3, self.data4
class US100UART:
distance = None
buf_dis = bytearray(2)
temperature = None
buf_temper = bytearray(1)
def __init__(self, port):
self.uart = UART(port, 9600)
self.uart.init(9600, bits=8, parity=None, stop=1, timeout=3000)
def isDistance(self):
if self.uart.any() and self.uart.any() % 2 == 0:
self.buf_dis = self.uart.read(2)
self.distance = (self.buf_dis[0] * 256) + self.buf_dis[1]
return True
else:
return False
async def read_distance(self):
"""
支持热插拔
:return: None
"""
self.uart.write(b'\x55')
while True:
await asyncio.sleep_ms(100)
if self.isDistance():
break
else:
await asyncio.sleep_ms(200)
if self.isDistance():
break
else:
self.distance = None
self.uart.read(self.uart.any())
self.uart.write(b'\x55')
async def read_temperature(self):
"""写着玩的"""
self.uart.write(b'\x50')
while True:
await asyncio.sleep_ms(100)
if self.uart.any():
self.buf_temper = self.uart.read(1)
self.temperature = self.buf_temper[0] - 45
break
else:
self.temperature = None
self.uart.write(b'\x50')
class Display():
def __init__(self, num, value):
self.uart = UART(num, value)
time.sleep(1)
def read_disp(self):
str = 'a'
if self.uart.any() != 0:
str = self.uart.read()
if str == b'begin':
return 3
elif str == b'end':
return 1
else:
return 0
def write_disp(self, strr, num):
a = 'main.' + strr + '.val=' + str(num)
self.uart.write(a)
self.uart.writechar(0xff)
self.uart.writechar(0xff)
self.uart.writechar(0xff)
time.sleep(0.03)
def write_d(self, n0, n1, n2, n3, n4, n5):
self.write_disp('n0', n0)
self.write_disp('n1', n1)
self.write_disp('n2', n2)
self.write_disp('n3', n3)
self.write_disp('n4', n4)
self.write_disp('n5', n5)
class USB_UART(IO_object):
def __init__(self, name):
self.uart = UART(1, 9600) # init with given baudrate
self.uart.init(9600, bits=8, parity=None, stop=1) # init with given parameters
self.buffer = bytearray(8)
self.name = name
assign_ID(self)
# Data acqisition variables
self.timer = pyb.Timer(available_timers.pop())
self.timestamp = fw.current_time
self.freq = -1.0
self.prev_freq = -1.0
def _timer_ISR(self, t):
if self.uart.any() > 0: # no message
self.uart.readinto(self.buffer, 2)
self.freq = int.from_bytes(self.buffer, 'little')
if self.freq != self.prev_freq:
self.timestamp = fw.current_time
interrupt_queue.put(self.ID)
self.prev_freq = self.freq
def _initialise(self):
self.timer.init(freq=100) # this should be 2*(client frequency)
self.timer.callback(self._timer_ISR)
def _process_interrupt(self):
fw.event_queue.put((self.timestamp, fw.event_typ, fw.events[self.name]))
def remote():
#initialise UART communication
uart = UART(6)
uart.init(9600, bits=8, parity = None, stop = 2)
# define various I/O pins for ADC
adc_1 = ADC(Pin('X19'))
adc_2 = ADC(Pin('X20'))
# set up motor with PWM and timer control
A1 = Pin('Y9',Pin.OUT_PP)
A2 = Pin('Y10',Pin.OUT_PP)
pwm_out = Pin('X1')
tim = Timer(2, freq = 1000)
motor = tim.channel(1, Timer.PWM, pin = pwm_out)
# Motor in idle state
A1.high()
A2.high()
speed = 0
DEADZONE = 5
# Use keypad U and D keys to control speed
while True: # loop forever until CTRL-C
while (uart.any()!=10): #wait we get 10 chars
n = uart.any()
command = uart.read(10)
if command[2]==ord('5'):
if speed < 96:
speed = speed + 5
print(speed)
elif command[2]==ord('6'):
if speed > - 96:
speed = speed - 5
print(speed)
if (speed >= DEADZONE): # forward
A1.high()
A2.low()
motor.pulse_width_percent(speed)
elif (speed <= -DEADZONE):
A1.low() # backward
A2.high()
motor.pulse_width_percent(-speed)
else:
A1.low() # idle
A2.low()
class JYMCU(object):
"""JY-MCU Bluetooth serial device driver. This is simply a light UART wrapper
with addition AT command methods to customize the device."""
def __init__(self, uart, baudrate):
""" uart = uart #1-6, baudrate must match what is set on the JY-MCU.
Needs to be a #1-C. """
self._uart = UART(uart, baudrate)
def __del__(self):
self._uart.deinit()
def any(self):
return self._uart.any()
def write(self, astring):
return self._uart.write(astring)
def writechar(self, achar):
self._uart.writechar(achar)
def read(self, num=None):
return self._uart.read(num)
def readline(self):
return self._uart.readline()
def readchar(self):
return self._uart.readchar()
def readall(self):
return self._uart.readall()
def readinto(self, buf, count=None):
return self._uart.readinto(buf, count)
def _cmd(self, cmd):
""" Send AT command, wait a bit then return result string. """
self._uart.write("AT+" + cmd)
udelay(500)
return self.readline()
def baudrate(self, rate):
""" Set the baud rate. Needs to be #1-C. """
return self._cmd("BAUD" + str(rate))
def name(self, name):
""" Set the name to show up on the connecting device. """
return self._cmd("NAME" + name)
def pin(self, pin):
""" Set the given 4 digit numeric pin. """
return self._cmd("PIN" + str(pin))
def version(self):
return self._cmd("VERSION")
def setrepl(self):
repl_uart(self._uart)
def keypad():
key = ('1','2','3','4','U','D','L','R')
uart = UART(6)
uart.init(9600, bits=8, parity = None, stop = 2)
while True:
while (uart.any()!=10): #wait we get 10 chars
n = uart.any()
command = uart.read(10)
key_index = command[2]-ord('1')
if (0 <= key_index <= 7) :
key_press = key[key_index]
if command[3]==ord('1'):
action = 'pressed'
elif command[3]==ord('0'):
action = 'released'
else:
action = 'nothing pressed'
print('Key',key_press,' ',action)
class SONIC():
def __init__(self, uart_port, uart_baud):
self.sonic = UART(uart_port, uart_baud)
def clear(self):
if self.sonic.any():
data = self.sonic.read(100)
def read(self):
if self.sonic.any():
data = self.sonic.read(10)
try:
data = data.decode()
except UnicodeError:
return -1
data = data[0:len(data) - 1]
return data
else:
return -1
class uart_tmcl_interface(tmcl_interface, tmcl_host_interface):
def __init__(self, port=3, data_rate=9600, host_id=2, module_id=1, debug=False):
tmcl_interface.__init__(self, host_id, module_id, debug)
tmcl_host_interface.__init__(self, host_id, module_id, debug)
self.__uart = UART(port, data_rate)
self.__uart.init(baudrate=data_rate, bits=8, parity=None, stop=1, timeout=10000, timeout_char=10000)
def __enter__(self):
return self
def __exit__(self, exitType, value, traceback):
del exitType, value, traceback
self.close()
def close(self):
self.__uart.deinit()
return 0;
def data_available(self, hostID=None, moduleID=None):
del hostID, moduleID
return self.__uart.any()
def _send(self, hostID, moduleID, data):
del hostID, moduleID
self.__uart.write(data)
def _recv(self, hostID, moduleID):
del hostID, moduleID
read = self.__uart.read(9)
return read
def printInfo(self):
pass
def enableDebug(self, enable):
self._debug = enable
@staticmethod
def supportsTMCL():
return True
@staticmethod
def supportsCANopen():
return False
@staticmethod
def available_ports():
return set([2, 3, 4])
class ESP8266(object):
def __init__(self):
self.uart = UART(6, 115200)
def write(self, command):
self.uart.write(command)
count = 5
while count >= 0:
if self.uart.any():
print(self.uart.readall().decode('utf-8'))
time.sleep(0.1)
count -= 1
class Scan():
def __init__(self):
self.scan = UART(3, 9600, read_buf_len=100)
def count(self):
return self.scan.any()
def receive(self):
time.sleep_ms(100)
count = self.count()
data = self.scan.read(count).decode()
return {'code': data}
class ESP8266(object):
def __init__(self):
self.uart = UART(6, 115200)
def write(self, command):
self.uart.write(command)
count = 5
while count >= 0:
if self.uart.any():
print(self.uart.readall().decode('utf-8'))
time.sleep(0.1)
count-=1
class UART_Port:
"""Implements a port which can send or receive commands with a bioloid
device using the pyboard UART class. This particular class takes
advantage of some features which are only available on the STM32F4xx processors.
"""
def __init__(self, uart_num, baud):
self.uart = UART(uart_num)
self.baud = 0
self.set_baud(baud)
base_str = 'USART{}'.format(uart_num)
if not hasattr(stm, base_str):
base_str = 'UART{}'.format(uart_num)
self.uart_base = getattr(stm, base_str)
# Set HDSEL (bit 3) in CR3 - which puts the UART in half-duplex
# mode. This connects Rx to Tx internally, and only enables the
# transmitter when there is data to send.
stm.mem16[self.uart_base + stm.USART_CR3] |= (1 << 3)
def any(self):
return self.uart.any()
def read_byte(self):
"""Reads a byte from the bus.
This function will return None if no character was read within the
designated timeout (set when we call self.uart.init).
"""
byte = self.uart.readchar()
if byte >= 0:
return byte
def set_baud(self, baud):
"""Sets the baud rate.
Note, the pyb.UART class doesn't have a method for setting the baud
rate, so we need to reinitialize the uart object.
"""
if self.baud != baud:
self.baud = baud
# The max Return Delay Time is 254 * 2 usec = 508 usec. The default
# is 500 usec. So using a timeout of 2 ensures that we wait for
# at least 1 msec before considering a timeout.
self.uart.init(baudrate=baud, timeout=2)
def write_packet(self, packet_data):
"""Writes an entire packet to the serial port."""
_write_packet(self.uart_base, packet_data, len(packet_data))
class mppt:
def __init__(self, port, baud=19200):
self.port = UART(port, baud)
self.data = {"here": 1}
def getdata(self):
while self.port.any() > 0:
try:
dataline = self.port.readline()
out = dataline.decode('UTF8')
split = ure.match("^(\w*)(\s*)(\w*)", out)
#self.data[split.group(1)] = split.group(3)
except Exception as e:
pass
return self.data
class Lcd():
def __init__(self):
self.lcd = UART(1, 9600)
def send_real(self, v, c, t):
if self.lcd.any():
self.lcd.read(self.lcd.any())
v = v.split('.')[0]
c = c.split('.')[0]
t = t.split('.')[0]
self.lcd.write('main.v.val=%s' % v)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.write('main.c.val=%s' % c)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.write('main.t.val=%s' % t)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.writechar(255)
def send_stand(self, vv, cc):
if self.lcd.any():
self.lcd.read(self.lcd.any())
vv = vv.split('.')[0]
cc = cc.split('.')[0]
self.lcd.write('main.vv.val=%s' % vv)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.write('main.cc.val=%s' % cc)
self.lcd.writechar(255)
self.lcd.writechar(255)
self.lcd.writechar(255)
class UART_Port:
"""Implements a port which can send or receive commands with a bioloid
device using the pyboard UART class. This particular class takes
advantage of some features which are only available on the STM32F4xx processors.
"""
def __init__(self, uart_num, baud):
self.uart = UART(uart_num)
self.baud = 0
self.set_baud(baud)
base_str = 'USART{}'.format(uart_num)
if not hasattr(stm, base_str):
base_str = 'UART{}'.format(uart_num)
self.uart_base = getattr(stm, base_str)
# Set HDSEL (bit 3) in CR3 - which puts the UART in half-duplex
# mode. This connects Rx to Tx internally, and only enables the
# transmitter when there is data to send.
stm.mem16[self.uart_base + stm.USART_CR3] |= (1 << 3)
def any(self):
return self.uart.any()
def read_byte(self):
"""Reads a byte from the bus.
This function will return None if no character was read within the
designated timeout (set when we call self.uart.init).
"""
byte = self.uart.readchar()
if byte >= 0:
return byte
def set_baud(self, baud):
"""Sets the baud rate.
Note, the pyb.UART class doesn't have a method for setting the baud
rate, so we need to reinitialize the uart object.
"""
if self.baud != baud:
self.baud = baud
# The max Return Delay Time is 254 * 2 usec = 508 usec. The default
# is 500 usec. So using a timeout of 2 ensures that we wait for
# at least 1 msec before considering a timeout.
self.uart.init(baudrate=baud, timeout=2)
def write_packet(self, packet_data):
"""Writes an entire packet to the serial port."""
_write_packet(self.uart_base, packet_data, len(packet_data))
class Mode():
def __init__(self):
#bluetooth communication
self.key = (0, 1, 2, 3, 'U', 'D', 'L', 'R')
self.uart = UART(6)
self.uart.init(9600, bits = 8, parity = None, stop = 2)
self.key_press = None
def loop(self):
if self.uart.any() > 5: #wait for a message to be sent
self.command = self.uart.read(5) #read the message which was sent
self.key_index = self.command[2] - ord('1') #convert ascii character send to an index for the list of keys
if 0 <= self.key_index <= 7: # checks the index is valid
self.key_press = self.key[self.key_index]
print("You've selected: ", str(self.key_press)) #print what is being pressed
class Network():
def __init__(self):
self.network = UART(6, 115200, read_buf_len=200)
def count(self):
return self.network.any()
def receive(self):
time.sleep_ms(100)
count = self.count()
data = self.network.read(count).decode()
data = data.strip()
return eval(data)
def send(self, data):
ss = json.dumps(data)
self.network.write(ss)
class JYMCU(object):
"""JY-MCU Bluetooth serial device driver. This is simply a light UART wrapper
with addition AT command methods to customize the device."""
def __init__( self, uart, baudrate ):
""" uart = uart #1-6, baudrate must match what is set on the JY-MCU.
Needs to be a #1-C. """
self._uart = UART(uart, baudrate)
def __del__( self ) : self._uart.deinit()
def any( self ) : return self._uart.any()
def write( self, astring ) : return self._uart.write(astring)
def writechar( self, achar ) : self._uart.writechar(achar)
def read( self, num = None ) : return self._uart.read(num)
def readline( self ) : return self._uart.readline()
def readchar( self ) : return self._uart.readchar()
def readall( self ) : return self._uart.readall()
def readinto( self, buf, count = None ) : return self._uart.readinto(buf, count)
def _cmd( self, cmd ) :
""" Send AT command, wait a bit then return result string. """
self._uart.write("AT+" + cmd)
udelay(500)
return self.readline()
def baudrate( self, rate ) :
""" Set the baud rate. Needs to be #1-C. """
return self._cmd("BAUD" + str(rate))
def name( self, name ) :
""" Set the name to show up on the connecting device. """
return self._cmd("NAME" + name)
def pin( self, pin ) :
""" Set the given 4 digit numeric pin. """
return self._cmd("PIN" + str(pin))
def version( self ) : return self._cmd("VERSION")
def setrepl( self ) : repl_uart(self._uart)
def pps_callback(line):
print("Updated GPS Object...")
global new_data # Use Global to trigger update
new_data = True
print('GPS Interrupt Tester')
# Instantiate the micropyGPS object
my_gps = MicropyGPS()
# Setup the connection to your GPS here
# This example uses UART 3 with RX on pin Y10
# Baudrate is 9600bps, with the standard 8 bits, 1 stop bit, no parity
# Also made the buffer size very large (1000 chars) to accommodate all the characters that stack up
# each second
uart = UART(3, 9600, read_buf_len=1000)
# Create an external interrupt on pin X8
pps_pin = pyb.Pin.board.X8
extint = pyb.ExtInt(pps_pin, pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, pps_callback)
# Main Infinite Loop
while 1:
# Do Other Stuff Here.......
# Update the GPS Object when flag is tripped
if new_data:
while uart.any():
my_gps.update(chr(uart.readchar())) # Note the conversion to to chr, UART outputs ints normally
print('UTC Timestamp:', my_gps.timestamp)
print('Date:', my_gps.date_string('long'))
print('Latitude:', my_gps.latitude_string())
print('Longitude:', my_gps.longitude_string())
print('Horizontal Dilution of Precision:', my_gps.hdop)
print()
new_data = False # Clear the flag
class Comm:
def __init__(self, _handler):
self.uart = UART(3, 115200, timeout_char=1000)
self.m_sendqueue = []
self.historic_package_sum = 0
self.pkgid = 0
self.pkg_start = False # Check whether a package start has received
self.length = -1 # Check package length
self.buf = [] # Buffer for incoming package
self.handler = _handler
def getPkgId(self):
t = self.historic_package_sum % 256
self.historic_package_sum += 1
return t
def queuePackage(self, pkg):
pkg["id"] = (self.historic_package_sum+1) % 256
self.m_sendqueue.append(pkg)
self.historic_package_sum += 1
return pkg["id"]
def sendPackageImmediate(self, pkg):
pkgid = pkg["id"]
pkg_type = pkg["type"]
data = pkg["data"]
pkgid %= 256
length = len(data) + 6
cs = 0xAA + length + pkg_type + pkgid + 0xFF
for b in data:
cs += b
cs %= 256
#print("pkg_len", length)
buf = bytes([0xAA, length, pkg_type, pkgid]) + data + bytes([cs, 0xFF])
# print('[SI]send immediate', buf, length, pkg_type, pkgid, cs, "[/SI]")
self.uart.write(buf)
def period(self):
for pkg in self.m_sendqueue:
self.sendPackageImmediate(pkg)
def listen(self):
# print("way, ", self.uart.any())
# return
while(self.uart.any() > 0):
ch = self.uart.read(1)
ch = ustruct.unpack("B", ch)[0] % 256
# print("ch",ch, "buffer", self.buf)
if ((not self.pkg_start) and ch == 0xAA):
# handle head
# print("handle head")
self.pkg_start = True
self.length = 0
self.buf.append(ch)
elif (self.pkg_start and self.length == 0):
# handle length
# print("handle len")
self.length = ch
self.buf.append(ch)
elif (self.pkg_start and len(self.buf) < self.length - 1):
# handle body
# print("handle body")
self.buf.append(ch)
elif (self.pkg_start and len(self.buf) == self.length - 1 and ch == 0xFF):
# handle tail
# print("handle tail")
self.buf.append(ch)
self.buildBufferPackage()
self.pkg_start = False
self.buf = []
#print("package recieved")
else:
# dump
# print("dump")
self.pkg_start = False # remove the package from handling
self.buf = []
def buildBufferPackage(self):
buf = bytes(self.buf)
_, _, pkg_type, pkgid = ustruct.unpack("<BBBB", buf)
pkg_type %= 256
pkgid %= 256
cs, _ = ustruct.unpack_from("<BB", buf, -2)
cs %= 256
pkg = {"type":pkg_type, "id":pkgid, "data": buf[4:-2]}
#print(self.buf, pkg_type, pkgid, cs, pkg["type"], pkg["id"])
if (pkg["type"] == PKGTYPE["kACK"]):
n_queue = []
for k, _pkg in enumerate(self.m_sendqueue):
if _pkg["id"] != pkgid:
n_queue.append(_pkg)
else:
n_queue += self.m_sendqueue[k+1:]
break
self.m_sendqueue = n_queue
elif (pkg["type"] != PKGTYPE["kACK"]):
self.sendPackageImmediate({
"id": pkgid,
"type": PKGTYPE["kACK"],
"data": b''
})
ESP_UART = UART(4,115200,timeout=100)
def sendToUart(msg):
ESP_UART.write(msg+'\r\n')
CWMODE_CUR = 'AT+CWMODE_CUR=1'
CWJAP_CUR = 'AT+CWJAP_CUR="TurnipSmart","turnip2016"'
CIPSTART = 'AT+CIPSTART="TCP","192.168.1.116",80'
CIPSEND = 'AT+CIPSEND=%s'
msg = 'Hello,I am TPYBoard.'
if __name__ == '__main__':
sendToUart('AT')
while True:
if ESP_UART.any() > 0:
buf = ESP_UART.read().decode().replace('\r','').replace('\n','').replace(' ','')
print(buf)
if buf.find('busyp') > -1 or buf.find('ERROR') > -1:
# AT指令执行失败
# 结束程序排查原因
break
elif buf.find('ATOK') > -1:
# 说明AT指令执行成功
# 成功进入AT指令模式
# 设置WIFI模式为sta模式
sendToUart(CWMODE_CUR)
elif buf.find(CWMODE_CUR) > -1:
# 设置sta模式成功，连接AP
sendToUart(CWJAP_CUR)
LED(1).on()
reason=upower.why() # motivo dell'uscita da low power mode.
# see upower.py module documentation.
uart.write(str(reason) +'\n')
#reason='ALARM_B' # solo per debug
try:
if reason=='X1':
verde.on()
pyb.delay(3)
verde.off()
uart.write('ready'+'\n') # uscito da standby - standby exit.
while test==0:
inBuffer_rx=""
if uart.any():
inBuffer_rx=uart.readline()
print(inBuffer_rx)
inBuffer_chr=""
if inBuffer_rx!='':
inBuffer_chr=inBuffer_rx.decode()
if inBuffer_chr=='connecting':
print('connecting')
uart.write('sono connesso!'+'\n') # uscito da standby - standby exit.
restore_data()
sa=leggi_sonda_a()
pkl['in_sonda_a']=int(sa)
sb=leggi_sonda_b()
pkl['in_sonda_b']=int(sb)
uart.write(str(pkl)+'\n') # invia dati a host - send data to host.
alpha = 0.92
pitch = 0
e_int = 0
e_diff = 0
v = 0
speed = 40
lt = 1
rt = 1
tic1 = pyb.micros()
while True:
dt = pyb.micros() - tic1
if dt > 5000:
if uart.any() != 5:
command = uart.read(5)
if command[3] == ord('1'):
if command[2] == oord('5'):
r += 1
print('1 pressed')
elif command[2] == oord('6'):
r -= 1
print('2 pressed')
elif command[2] == ord('7'):
r += 0.7
lt = -1
print('3 pressed')
elif command[2] == ord('8'):
r += 0.6
rt = -1
indicator = pyb.LED(4)
increase = True
sensor_queue = SensorQueue(tmp36,
mcp9808,
accel,
gps
)
command_pool = CommandPool(servo1, motor_a)
communicator = Communicator(sensor_queue, command_pool)
while True:
if new_data:
while uart.any():
gps.update(chr(uart.readchar()))
new_data = False
communicator.write_packet()
communicator.read_command()
if increase:
indicator.intensity(indicator.intensity() + 5)
else:
indicator.intensity(indicator.intensity() - 5)
if indicator.intensity() <= 0:
increase = True
elif indicator.intensity() >= 255:
ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width=1305)
ch2 = tim2.channel(1, Timer.PWM, pin=Pin("P6"), pulse_width_percent=int(speed))
pin1 = Pin('P1', Pin.OUT_PP, Pin.PULL_NONE)
pin9 = Pin('P9', Pin.OUT_PP, Pin.PULL_NONE)
pin3 = Pin('P3', Pin.OUT_PP, Pin.PULL_NONE)
pin8 = Pin('P8', Pin.OUT_PP, Pin.PULL_NONE)
pin1.value(1)
pin9.value(0)
pin3.value(1)
pin8.value(1)
uart = UART(3, 115200)
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
if uart.any() > 0:
char = uart.read(1)
if char == b's':
speed = 0
elif char == b'r':
if uart.any() > 1:
temp = uart.read(2)
speed = int(temp.decode("utf-8"))
pin9.value(0)
uart.write(str(speed))
uart.write('\n')
elif char == b'p':
if uart.any() > 1:
from pyb import UART
uart = UART(1)
uart = UART(1, 9600)
uart = UART(1, 9600, bits=8, parity=None, stop=1)
print(uart)
uart.init(2400)
print(uart)
print(uart.any())
print(uart.write('123'))
print(uart.write(b'abcd'))
print(uart.writechar(1))
# make sure this method exists
uart.sendbreak()
class Receiver:
# Allowed System Field values
_DSM2_1024_22MS = 0x01
_DSM2_2048_11MS = 0x12
_DSMS_2048_22MS = 0xa2
_DSMX_2048_11MS = 0xb2
_SYSTEM_FIELD_VALUES = (_DSM2_1024_22MS, _DSM2_2048_11MS, _DSMS_2048_22MS, _DSMX_2048_11MS)
# Channel formats depending on system field value
_MASK_1024_CHANID = 0xFC00 # when 11ms
_MASK_1024_SXPOS = 0x03FF # when 11ms
_MASK_2048_CHANID = 0x7800 # when 22ms
_MASK_2048_SXPOS = 0x07FF # when 22ms
# Serial configuration
_uart = None
_uart_port = 0 # Which UART port to use?
_uart_speed = 0 # Which UART speed to use?
# Serial buffer and frame data
_system_field = None
_frame = [0] * 16 # Assumption: frames are received correctly, no need of intermediate buffer and controls
_channels = [0] * 20 # Up-to 20 channels can be used by SPM4648
_debug = False
# ########################################################################
# ### Properties
# ########################################################################
@property
def port(self):
return self._uart_port
@property
def speed(self):
return self._uart_speed
@property
def frame(self):
return self._frame
@property
def channels(self):
return self._channels
@property
def system_field(self):
return self._system_field
# ########################################################################
# ### Constructor and destructor
# ########################################################################
def __init__(self, port, speed, debug=False):
self._debug = debug
self._uart_port = port
self._uart_speed = speed
self._uart = UART(self._uart_port, self._uart_speed)
# ########################################################################
# ### Functions
# ########################################################################
def read_serial(self):
# Lire un frame
if self._uart.any():
index = 0
while index < 16:
self._frame[index] = self._uart.readchar()
index += 1
self._decode_frame()
return True
else:
return False
def _decode_frame(self):
# Verify the system field (_channels[2])
if self._frame[1] in self._SYSTEM_FIELD_VALUES:
self._system_field = self._frame[1]
if self._frame[1] == self._DSM2_1024_22MS:
for i in range(1, 7):
data = self._frame[i * 2] * 256 + self._frame[(i * 2) + 1]
channel = (data & self._MASK_1024_CHANID) >> 10
value = data & self._MASK_1024_SXPOS
self._channels[channel] = value
else:
for i in range(1, 7):
data = self._frame[i * 2] * 256 + self._frame[(i * 2) + 1]
channel = (data & self._MASK_2048_CHANID) >> 11
value = data & self._MASK_2048_SXPOS
self._channels[channel] = value
else:
pass # Invalid system field value -> Do nothing
if self._debug:
self.debug()
def debug(self):
if not self._debug:
return
print("RX OUT: ", end="")
for i in range(0, 4):
print("CH%2d: %4d" % (i, self._channels[i]),
end=" || ")
print("")
# with open('/sd/log.csv', 'a') as log:
# log.write('\n\n-----------------------------\n\n')
xbee.write('\nprogram initiated at: {}'.format(my_gps.timestamp))
xbee.write('\ntarget point: {}'.format(finish_point))
xbee.write('\nlaunching at: {}'.format(launch_point))
# Landing-check loop
sw = pyb.Switch()
while True:
# Update the GPS Object when flag is tripped
# if sw():
# log.close()
# break
if new_data:
while uart.any():
my_gps.update(chr(uart.readchar()))
if my_gps.latitude[0] != 0:
xbee.write('\nChecking GPS and altitude to see if landed...')
initial_lat = convert_latitude(my_gps.latitude)
initial_long = convert_longitude(my_gps.longitude)
initial_point = (initial_lat, initial_long)
xbee.write('\nCurrent point: {}'.format(initial_point))
dist_from_launch = calculate_distance(launch_point, initial_point)
xbee.write('\nDistance from launch: {}'.format(dist_from_launch))
# acceptable distance is currently set for simple testing
if dist_from_launch < acceptable_dist_from_launch:
# pyb.stop()
pyb.delay(100)
elif dist_from_launch > acceptable_dist_from_launch:
altitude_1 = my_gps.altitude
RPS = 0 # Comment this out if you use speed PID w/ Hall Effect, though this probably doesn't matter w/ interrupts
go_check = 0
start = pyb.millis() # Used for RPS calculation
speed_pulse_width = 20
while(True):
# STOP CODE (measures distance to stop by counting rotations of wheel using Hall Effect interrupt)
# We have a better stop code using "blobs" (SEE VIKRAM'S CODE FOR CHECKOFF 2!!!)
if(counter > 440):
inA.high()
inB.high()
# BLUETOOTH CONTROL
if uart.any():
print("UART")
cmd = (str)(bytes.decode(uart.read()))
print(cmd)
if cmd == "g": #go
counter = 0
inA.high()
inB.low()
go_check = 1
cmd = "x"
elif cmd == "q": #hard brake
inA.high()
inB.high()
class ESP8266(object):
def __init__(self, uart=1, baud_rate=115200):
"""Initialize this module. uart may be an integer or an instance
of pyb.UART. baud_rate can be used to set the Baud rate for the
serial communication."""
if uart:
if type(uart) is int:
self.uart = UART(uart, baud_rate)
elif type(uart) is UART:
self.uart = uart
else:
raise Exception(
"Argument 'uart' must be an integer or pyb.UART object!")
else:
raise Exception("Argument uart must not be 'None'!")
def _send_command(self, cmd, timeout=0, debug=False):
"""Send a command to the ESP8266 module over UART and return the
output.
After sending the command there is a 1 second timeout while
waiting for an anser on UART. For long running commands (like AP
scans) there is an additional 3 seconds grace period to return
results over UART.
Raises an CommandError if an error occurs and an CommandFailure
if a command fails to execute."""
if debug:
start = micros()
cmd_output = []
okay = False
if cmd == '' or cmd == b'':
raise CommandError("Unknown command '" + cmd + "'!")
# AT commands must be finalized with an '\r\n'
cmd += '\r\n'
if debug:
print("%8i - TX: %s" % (elapsed_micros(start), str(cmd)))
self.uart.write(cmd)
# wait at maximum one second for a command reaction
cmd_timeout = 100
while cmd_timeout > 0:
if self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
if debug:
print("%8i - 'OK' received!" % (elapsed_micros(start)))
okay = True
delay(10)
cmd_timeout -= 1
if cmd_timeout == 0 and len(cmd_output) == 0:
if debug == True:
print("%8i - RX timeout of answer after sending AT command!" %
(elapsed_micros(start)))
else:
print("RX timeout of answer after sending AT command!")
# read output if present
while self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
if debug:
print("%8i - 'OK' received!" % (elapsed_micros(start)))
okay = True
# handle output of AT command
if len(cmd_output) > 0:
if cmd_output[-1].rstrip() == b'ERROR':
raise CommandError('Command error!')
elif cmd_output[-1].rstrip() == b'OK':
okay = True
elif not okay:
# some long running commands do not return OK in case of success
# and/or take some time to yield all output.
if timeout == 0:
cmd_timeout = 300
else:
if debug:
print("%8i - Using RX timeout of %i ms" %
(elapsed_micros(start), timeout))
cmd_timeout = timeout / 10
while cmd_timeout > 0:
delay(10)
if self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
okay = True
break
elif cmd_output[-1].rstrip() == b'FAIL':
raise CommandFailure()
cmd_timeout -= 1
if not okay and cmd_timeout == 0 and debug:
print("%8i - RX-Timeout occured and no 'OK' received!" %
(elapsed_micros(start)))
return cmd_output
@classmethod
def _join_args(cls, *args, debug=True):
"""Joins all given arguments as the ESP8266 needs them for the
argument string in a 'set' type command.
Strings must be quoted using '"' and no spaces outside of quoted
srrings are allowed."""
while type(args[0]) is tuple:
if len(args) == 1:
args = args[0]
if debug:
print(args)
str_args = []
for arg in args:
if type(arg) is str:
str_args.append('"' + arg + '"')
elif type(arg) is bytes:
str_args.append(arg.decode())
elif type(arg) is bool:
str_args.append(str(int(arg)))
else:
str_args.append(str(arg))
if debug:
print(str_args)
return ','.join(str_args).encode()
@classmethod
def _parse_accesspoint_str(cls, ap_str):
"""Parse an accesspoint string description into a hashmap
containing its parameters. Returns None if string could not be
split into 3 or 5 fields."""
if type(ap_str) is str:
ap_str = ap_str.encode()
ap_params = ap_str.split(b',')
if len(ap_params) == 5:
(enc_mode, ssid, rssi, mac, channel) = ap_params
ap = {
'encryption_protocol': int(enc_mode),
'ssid': ssid,
'rssi': int(rssi),
'mac': mac,
'channel': int(channel)
}
elif len(ap_params) == 3:
(enc_mode, ssid, rssi) = ap_params
ap = {
'encryption_protocol': int(enc_mode),
'ssid': ssid,
'rssi': int(rssi),
}
else:
ap = None
return ap
'''
@classmethod
def _parse_station_ip(string)
if type(string) is str:
return string
else:
return None
'''
def _query_command(self, cmd, timeout=0, debug=False):
"""Sends a 'query' type command and return the relevant output
line, containing the queried parameter."""
return self._send_command(cmd + b'?', timeout=timeout,
debug=debug)[1].rstrip()
def _set_command(self, cmd, *args, timeout=0, debug=False):
"""Send a 'set' type command and return all lines of the output
which are not command echo and status codes.
This type of AT command usually does not return output except
the echo and 'OK' or 'ERROR'. These are not returned by this
method. So usually the result of this methid must be an empty list!"""
return self._send_command(cmd + b'=' +
ESP8266._join_args(args, debug=debug),
timeout=timeout,
debug=debug)[1:-2]
def _execute_command(self, cmd, timeout=0, debug=False):
"""Send an 'execute' type command and return all lines of the
output which are not command echo and status codes."""
return self._send_command(cmd, timeout=timeout, debug=debug)[1:-2]
def test(self, debug=False):
"""Test the AT command interface."""
return self._execute_command(CMDS_GENERIC['TEST_AT'],
debug=debug) == []
def version(self, debug=False):
"""Test the AT command interface."""
return self._execute_command(CMDS_GENERIC['VERSION_INFO'],
debug=debug) == []
def reset(self, debug=False):
"""Reset the module and read the boot message.
ToDo: Interpret the boot message and do something reasonable with
it, if possible."""
boot_log = []
if debug:
start = micros()
self._execute_command(CMDS_GENERIC['RESET'], debug=debug)
# wait for module to boot and messages appearing on self.uart
timeout = 300
while not self.uart.any() and timeout > 0:
delay(10)
timeout -= 1
if debug and timeout == 0:
print("%8i - RX timeout occured!" % (elapsed_micros(start)))
# wait for messages to finish
timeout = 300
while timeout > 0:
if self.uart.any():
boot_log.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(boot_log[-1])))
delay(20)
timeout -= 1
if debug and timeout == 0:
print("%8i - RTimeout occured while waiting for module to boot!" %
(elapsed_micros(start)))
return boot_log[-1].rstrip() == b'ready'
def get_mode(self):
"""Returns the mode the ESP WIFI is in:
1: station mode
2: accesspoint mode
3: accesspoint and station mode
Check the hashmap esp8266.WIFI_MODES for a name lookup.
Raises an UnknownWIFIModeError if the mode was not a valid or
unknown.
"""
mode = int(self._query_command(CMDS_WIFI['MODE']).split(b':')[1])
if mode in WIFI_MODES.keys():
return mode
else:
raise UnknownWIFIModeError("Mode '%s' not known!" % mode)
def set_mode(self, mode, debug=False):
"""Set the given WIFI mode.
Raises UnknownWIFIModeError in case of unknown mode."""
if mode not in WIFI_MODES.keys():
raise UnknownWIFIModeError("Mode '%s' not known!" % mode)
return self._set_command(CMDS_WIFI['MODE'], mode, debug=debug)
def get_accesspoint(self, debug=False):
"""Read the SSID of the currently joined access point.
The SSID 'No AP' tells us that we are not connected to an access
point!"""
answer = self._query_command(CMDS_WIFI["CONNECT"], debug=debug)
#print("Answer: " + str(answer))
if answer == b'No AP':
result = None
else:
result = answer.split(b'+' + CMDS_WIFI['CONNECT'][3:] +
b':')[1][1:-1]
return result
def connect(self, ssid, psk, debug=False):
"""Tries to connect to a WIFI network using the given SSID and
pre shared key (PSK). Uses a 20 second timeout for the connect
command.
Bugs: AT firmware v0.21 has a bug to only join a WIFI which SSID
is 10 characters long."""
self._set_command(CMDS_WIFI['CONNECT'],
ssid,
psk,
debug=debug,
timeout=20000)
def disconnect(self, debug=False):
"""Tries to connect to a WIFI network using the given SSID and
pre shared key (PSK)."""
return self._execute_command(CMDS_WIFI['DISCONNECT'],
debug=debug) == []
@classmethod
def _parse_list_ap_results(cls, ap_scan_results):
aps = []
for ap in ap_scan_results:
try:
ap_str = ap.rstrip().split(CMDS_WIFI['LIST_APS'][-4:] +
b':')[1].decode()[1:-1]
except IndexError:
# Catching this exception means the line in scan result
# was probably rubbish
continue
# parsing the ap_str may not work because of rubbish strings
# returned from the AT command. None is returned in this case.
ap = ESP8266._parse_accesspoint_str(ap_str)
if ap:
aps.append(ap)
return aps
def list_all_accesspoints(self, debug=False):
"""List all available access points.
TODO: The IoT AT firmware 0.9.5 seems to sporadically yield
rubbish or mangled AP-strings. Check needed!"""
return ESP8266._parse_list_ap_results(
self._execute_command(CMDS_WIFI['LIST_APS'], debug=debug))
def list_accesspoints(self, *args):
"""List accesspoint matching the parameters given by the
argument list.
The arguments may be of the types string or integer. Strings can
describe MAC adddresses or SSIDs while the integers refer to
channel names."""
return ESP8266._parse_list_ap_results(
self._set_command(CMDS_WIFI['LIST_APS'], args))
def set_accesspoint_config(self,
ssid,
password,
channel,
encrypt_proto,
debug=False):
"""Configure the parameters for the accesspoint mode. The module
must be in access point mode for this to work.
After setting the parameters the module is reset to
activate them.
The password must be at least 8 characters long up to a maximum of
64 characters.
WEP is not allowed to be an encryption protocol.
Raises CommandFailure in case the WIFI mode is not set to mode 2
(access point) or 3 (access point and station) or the WIFI
parameters are not valid."""
if self.get_mode() not in (2, 3):
raise CommandFailure('WIFI not set to an access point mode!')
if type(ssid) is not str:
raise CommandFailure('SSID must be of type str!')
if type(password) is not str:
raise CommandFailure('Password must be of type str!')
if len(password) > 64 or len(password) < 8:
raise CommandFailure('Wrong password length (8..64)!')
if channel not in range(1, 15) and type(channel) is not int:
raise CommandFailure('Invalid WIFI channel!')
if encrypt_proto not in (0, 2, 3, 4) or type(encrypt_proto) is not int:
raise CommandFailure('Invalid encryption protocol!')
self._set_command(CMDS_WIFI['AP_SET_PARAMS'],
ssid,
password,
channel,
encrypt_proto,
debug=debug)
self.reset()
def get_accesspoint_config(self):
"""Reads the current access point configuration. The module must
be in an acces point mode to work.
Returns a hashmap containing the access point parameters.
Raises CommandFailure in case of wrong WIFI mode set."""
if self.get_mode() not in (2, 3):
raise CommandFailure('WIFI not set to an access point mode!')
(ssid, password, channel, encryption_protocol) = self._query_command(
CMDS_WIFI['AP_SET_PARAMS'], debug=False).split(b':')[1].split(b',')
return {
'ssid': ssid,
'password': password,
'channel': int(channel),
'encryption_protocol': int(encryption_protocol)
}
def list_stations(self):
"""List IPs of stations which are connected to the access point.
ToDo: Parse result and return python list of IPs (as str)."""
return self._execute_command(CMDS_WIFI['AP_LIST_STATIONS'],
debug=False)
def set_dhcp_config(self, mode, status, debug=False):
"""Set the DHCP configuration for a specific mode.
Oddities:
The mode seems not to be the WIFI mode known from the methods
set_mode() and get_mode(). The mode are as follows according to
the Esspressif documentation:
0: access point (softAP)
1: station
2: access point and station
The second argument (status) is strange as well:
0: enable
1: disable
"""
# Invert status to make the call to this methid reasonable.
if type(status) is int:
status = bool(status)
if type(status) is bool:
status = not status
return self._set_command(CMDS_WIFI['DHCP_CONFIG'],
mode,
status,
debug=debug)
def set_autoconnect(self, autoconnect, debug=False):
"""Set if the module should connnect to an access point on
startup."""
return self._set_command(CMDS_WIFI['SET_AUTOCONNECT'],
autoconnect,
debug=debug)
def get_station_ip(self, debug=False):
"""get the IP address of the module in station mode.
The IP address must be given as a string. No check on the
correctness of the IP address is made."""
return self._query_command(CMDS_WIFI['SET_STATION_IP'], debug=debug)
def set_station_ip(self, ip_str, debug=False):
"""Set the IP address of the module in station mode.
The IP address must be given as a string. No check on the
correctness of the IP address is made."""
return self._set_command(CMDS_WIFI['SET_STATION_IP'],
ip_str,
debug=debug)
def get_accesspoint_ip(self, debug=False):
"""get the IP address of the module in access point mode.
The IP address must be given as a string. No check on the
correctness of the IP address is made."""
return self._query_command(CMDS_WIFI['SET_AP_IP'], debug=debug)
def set_accesspoint_ip(self, ip_str, debug=False):
"""Set the IP address of the module in access point mode.
The IP address must be given as a string. No check on the
correctness of the IP address is made."""
return self._set_command(CMDS_WIFI['SET_AP_IP'], ip_str, debug=debug)
def get_connection_status(self):
"""Get connection information.
ToDo: Parse returned data and return python data structure."""
return self._execute_command(CMDS_IP['STATUS'])
def start_connection(self, protocol, dest_ip, dest_port, debug=False):
"""Start a TCP or UDP connection.
ToDo: Implement MUX mode. Currently only single connection mode is
supported!"""
self._set_command(CMDS_IP['START'],
protocol,
dest_ip,
dest_port,
debug=debug)
def send(self, data, debug=False):
"""Send data over the current connection."""
self._set_command(CMDS_IP['SEND'], len(data), debug=debug)
print(b'>' + data)
self.uart.write(data)
def ping(self, destination, debug=False):
"""Ping the destination address or hostname."""
return self._set_command(CMDS_IP['PING'], destination, debug=debug)
ExtInt('Y1', ExtInt.IRQ_RISING_FALLING, Pin.PULL_NONE,
cfreq_R) #hall sensors as inperrupts
ExtInt('Y2', ExtInt.IRQ_RISING_FALLING, Pin.PULL_NONE, cfreq_L)
ExtInt(Pin('Y3'), ExtInt.IRQ_RISING, Pin.PULL_DOWN,
brake_intr) # external intrupt to have sudden brake
pyb.Timer(4, freq=33.33).callback(isr_speed_timer)
#############################################################################################################
# LOOP #
#############################################################################################################
while True:
if (brake_status and status):
motor1_switch.high()
motor2_switch.high()
status = False
if (uart.any()):
uart_input_text = uart.read()
print(uart_input_text)
uart_input_text = uart_input_text[0:-1]
contr_str = uart_input_text.decode("utf-8")
print(contr_str)
if (int(contr_str[0:4]) == control_dict["__START__"]):
brake_status = False
motor_L_brake.high()
motor_R_brake.high()
ct = 1
start()
if (int(contr_str[0:4]) == control_dict["__TURNLEFT__"]):
left()
if (int(contr_str[0:4]) == control_dict["__TURNRIGHT__"]):
right()
class HA7S:
""" Class for 1-wire master using HA7S.
Contains drivers for:
DS18B20 temp sensor
Display controller PIC for LCD
DS2423 Counter (2 channels 32 bits counters)
"""
def __init__(self, uart_port):
self.ROW_LENGTH = 20 # LCD 4 rows x 20 chars
self.uart = UART(uart_port, 9600)
def scan_for_devices(self):
""" Find all 1-Wire rom_codes on the bus """
strt = micros()
rom_codes = [] # Prepare list with Units found on the bus
r = "" # Return string
r = self.tx_rx('S', 17) # Search for first device
if len(r) > 1:
rom_codes.append(r[:-1]) # Add to list (Skip final <cr>)
##print("Första enhet: ", rom_codes)
while True:
# delay(100)
r = self.tx_rx('s', 17) # Search next rom_codes todo: ger timeout sista gången
if len(r) > 1:
rom_codes.append(r[:-1]) # Add to list (Skip final <cr>)
else:
break
# print("Enheter: ", rom_codes)
##print("Scan for devices: ", elapsed_micros(strt) / 1e6, 's')
return rom_codes
def read_ds18b20_temp(self, rom):
""" Setup and read temp data from DS18b20 """
# Todo: check negative temps works
retries = 3
while retries:
""" Initiate Temperature Conversion by selecting and sending 0x44-command """
resp = self.tx_rx(b'A' + rom + '\r', 17) # Adressing
dummy = self.tx_rx('W0144\r', 3) # Write block of data '44' Trigg measurement
resp1 = self.tx_rx('M\r', 17) # Reset AND reselect (enl HA7S doc)
if resp1 == resp:
delay(750) # Give DS18B20 time to measure
# The temperature result is stored in the scratchpad memory
data = self.tx_rx(b'W0ABEFFFFFFFFFFFFFFFFFF\r', 21) # Write to scratchpad and READ result
dummy = self.tx_rx('R', 1) # Reset
m = self.hex_byte_to_int(data[4:6])
l = self.hex_byte_to_int(data[2:4])
t = (m << 8) | (l & 0xff)
if m < 8:
t *= 0.0625 # Convert to Temp [°C]; Plus
else:
# temp given as 2's compl 16 bit int
t = (t - 65536) * 0.0625 # Convert to Temp [°C]; Minus
print("Rom, retur temperatur: ", rom, data, t, '°C')
return t
else:
retries -= 1
print("Retries: resp, resp1: ", retries, resp, resp1)
if retries < 1:
break
def read_ds2423_counters(self, rom):
""" Read counter values for the two counters (A & B) conncted to external pins """
resp = self.tx_rx(b'A' + rom + '\r', 17) # Adressing
""" Write/read block: A5 01C0/01E0(CounterA/B) [MSByte sent last] + 'FF'*42 (timeslots during which slave
returns 32 bytes scratchpad data + 4(cntA/cntB) + 4(zeroBytes) + 2 CRC bytes """
# Todo: check if respons = written; repeat otherwise
# We set adr so we only read LAST byte of page 14. We also get counter(4 B) + zerobytes(4 B) and CRC(2 B)
dataA = self.tx_rx('W0EA5DF01' + 'FF' * 11 + '\r', 29) # Read mem & Counter + TA1/TA2 (adr = 0x11C0)
dummy = self.tx_rx('M\r', 17) # Reset AND reselect (enl HA7S doc)
# We set adr so we only read LAST byte of page 15. We also get counter(4 B) + zerobytes(4 B) and CRC(2 B)
dataB = self.tx_rx('W0EA5FF01' + 'FF' * 11 + '\r', 29) # Read mem & Counter + TA1/TA2 (adr = 0x11C0)
dummy = self.tx_rx('R\r', 1) # Reset and red data (b'BE66014B467FFF0A102D\r')
''' Convert 32 bits hexadecimal ascii-string (LSByte first) to integer '''
cntA = self.lsb_first_hex_ascii_to_int32(dataA[8:16])
cntB = self.lsb_first_hex_ascii_to_int32(dataB[8:16])
print("ReadCnt: ", cntA, cntB, dataA, dataB)
return (cntA, cntB)
def write_ds2423_scratchpad(self, rom, s, target_adr):
""" Write to Scratchpad (max 32 bytes)
target_adr [0..0x1FF] as integer
Not implemented: readback of CRC after end of write. This works ONLY if data
written extends to end of page """
self.tx_rx(b'A' + rom + '\r', 17) # Adressing
""" Write to scratach: 'OF' [TA1/TA2] (Byte reversed) + data string as hex ascii
HA7S can only write 32 bytes in a chunk. """
# Todo: check if respons = written; repeat otherwise
# Check string length AND that it fits on page (check target_adr + s_len)
s_len = len(s)
if target_adr < 0x200 and s_len <= 32 and (target_adr % 0x20) + s_len <= 32:
swap_adr = self.int8_to_2hex_string(target_adr & 0xFF) + self.int8_to_2hex_string(target_adr >> 8)
if s_len > 29:
''' Send first 16 bytes only, as first part '''
resp1 = self.tx_rx('W130F' + swap_adr + self.bin2hex(s[:16]) + '\r', 7 + 32) # First 16 bytes of data
''' Adjust parameters for next write '''
nr_bytes_hex = self.int8_to_2hex_string(s_len - 16)
s = s[16:]
s_len -= 16
# Send rest of string as second part
resp2 = self.tx_rx('W' + nr_bytes_hex + self.bin2hex(s) + '\r', 1 + (s_len * 2)) # Only string now
resp = resp1[:6] + ':' + resp1[6:-1] + '/' + resp2
else:
nr_bytes_hex = self.int8_to_2hex_string(3 + s_len)
resp = self.tx_rx('W' + nr_bytes_hex + '0F' + swap_adr + self.bin2hex(s) + '\r', 7 + (s_len * 2))
dummy = self.tx_rx('R\r', 1) # Reset and stop
print("write_ds2423_scratchpad: Reponse", resp)
else:
print("write_ds2423_scratchpad: String will not fit!; Target_adr or length of string to big!")
def read_and_copy_ds2423_scratchpad(self, rom):
""" Read Scratchpad and copy to SRAM
First TA1/TA2 and 'Ending offset E/S' is fetched from scratchpad contents and then
the offset inside scratchpad is set to 5 lsbits of TA1, and data is fetched from there until 'Ending offset'.
'Ending offset' is the offset for the last chr written to scratchpad
Finally a copy of (updated part of) scratchpad is written to SRAM """
# Todo: check if respons = written; repeat otherwise
self.tx_rx(b'A' + rom + '\r', 17) # Adressing
auth = self.tx_rx('W04AA' + ('FF' * 3) + '\r', 9) # Read 'AA' + TA1/TA2 + Status(E/S)
target_adr = (self.hex_byte_to_int(auth[4:6]) << 8) + self.hex_byte_to_int(auth[2:4]) # MSB and LSB Swapped
status = self.hex_byte_to_int(auth[6:8])
print(" read_and_copy_ds2423_scratchpad: auth, targetAdress, Status(E/S) 'Ending offset': ",
auth, hex(target_adr), hex(status & 0x1F))
''' Continue reading timeslots until end of written part of scratchpad '''
nr_bytes = (status & 0x1F) - (target_adr & 0x1F) + 1 # 1+Ending offset-Start offset = # bytes written/to read
nr_bytes_hex = self.int8_to_2hex_string(nr_bytes)
data = self.tx_rx('W' + nr_bytes_hex + ('FF' * nr_bytes) + '\r', (2 * nr_bytes) + 1) # Read rest of chars
dummy = self.tx_rx('M\r', 17) # Reset and adress again
""" Write Copy scratchpad command: copy scratchpad to memory –– Authenticate with previous TA1/TA2 + E/S """
a = list(auth[2:-1])
s = ''
for b in a:
s += chr(b)
##print("auth: ", a, chr(b), s)
resp = self.tx_rx('W045A' + s + '\r', 9) # Copy Scratch: '5A' + TA1/TA2 + Status(E/S)
dummy = self.tx_rx('R\r', 1) # Reset and stop
print(" read_and_copy_ds2423_scratchpad: Repons: ", resp[:-1], ':', data, nr_bytes)
return data
def read_ds2423_mem(self, rom, page):
""" Read Memory page (32 bytes)
page = page-number as integer [0..15]; the whole page is read.
If reading includes the last byte in a page, DS2423 also sends counter value(4 bytes) + 12 bytes more
Reading can continue into next page, BUT HA7S can only read 32 bytes in a chunk. """
# Todo: check if respons = written; repeat otherwise
page_adr = self.int16_to_4hex_string((page % 16) * 0x20)
page_swap = page_adr[2:] + page_adr[:2] # Swap MSB and LSB
resp = self.tx_rx(b'A' + rom + '\r', 17) # Adressing
""" Write/read block: A5 01C0/01E0(CounterA/B) [or ANY PAGE (= adr)] + 'FF'*42 (timeslots during which slave
returns 32 ramData + 4(cntA/cntB) + 2(0) + 2 CRC bytes; All data as hex ascii (Byte reversed) """
data1 = self.tx_rx('W13F0' + page_swap + 'FF' * 16 + '\r', 39) # Read mem + TA1/TA2 (adr = 0x01E0)
''' We can continue sending timeslots for reading data until we send Reset '''
data2 = self.tx_rx('W10' + 'FF' * 16 + '\r', 33) # Continue fetching ram data
dummy = self.tx_rx('R\r', 1) # Reset and stop reading
##print("Repons: ", resp, data1, data2)
d = data1[6:-1] + data2[:-1] # Skip respons, cmd, TA1, TA2 and '\r'
s = self.hex_bytes_to_str(d)
return (d, s)
def lcd_init(self, rom, use_custom_chars=True):
""" Init LCD with custom chr generator """
if use_custom_chars:
# Load Character generator into user area of CG-RAM
# chr0 = [0b10001, 0b01111]
chr0 = [0b01010, 0b00000, 0b00100, 0b01010, 0b11111, 0b10001, 0b10001, 0b00000, # 'Ä' {'ä' finns i #225}
0b01010, 0b00000, 0b01110, 0b10001, 0b10001, 0b10001, 0b01110, 0b00000, # 'Ö' {'ö' finns i #239}
0b00100, 0b01010, 0b00100, 0b01110, 0b10001, 0b11111, 0b10001, 0b00000, # 'Å'
0b00100, 0b01010, 0b00100, 0b01110, 0b10001, 0b10001, 0b01111, 0b00000, # 'å'
0b01100, 0b10010, 0b10010, 0b01100, 0b00000, 0b00000, 0b00000, 0b00000, # '°'
0b00000, 0b00000, 0b01111, 0b10001, 0b10001, 0b01111, 0b00001, 0b01110, # 'g'
0b01010, 0b00000, 0b01110, 0b00001, 0b01110, 0b10001, 0b01111, 0b00000, # 'ä' finns i #225
0b00000, 0b01010, 0b00000, 0b01110, 0b10001, 0b10001, 0b01110, 0b00000] # 'ö' finns i #239
''' First adress PIC via HA7Scommand "A" '''
dummy = self.tx_rx(b'A' + rom + b'\r', 17) # Adressing
dummy = self.tx_rx('W021040\r', 5) # Write 0x40 directly to LCD register: Set start adr = 0 in CG-RAM
delay(1)
dummy = self.tx_rx('M\r', 17) # Reset AND reselect
for c in chr0: # Send chr0 to LCD CG-RAM (max 8 chrs á 8 bytes)
dummy = self.tx_rx('W0212' + self.bin2hex(chr(c)) + '\r', 5) # Write font (chr0) to LCD CG-RAM
delay(1)
dummy = self.tx_rx('M\r', 17) # Reset AND reselect
# Switch back to pointing to DDRAM (NOT CGRAM), else will clobber CGRAM !!
# self.hal_write_command(0x80 | 0) # Set start adr = 0
dummy = self.tx_rx('W021080\r', 5) # Write 0x40 directly to LCD register memory: Point to CDDRAM
delay(1)
dummy = self.tx_rx('R\r', 17) # Reset
def print_on_lcd(self, rom, msg, row_nr, col=0, clear_LCD=False, use_custom_chars=False):
""" Send text message to scratchpad memory in PIC with HA7S Write/Read block cmd
then copy from scratchpad to LCD
N.B. swedish char åäöÅÄÖ and other non US-ASCII charas are sent as UTF-8 (2 chars)
entries in user_char with chr >127, does not work? """
user_char = {'Ä': chr(0), 'Ö': chr(1), 'Å': chr(2), 'å': chr(3), # Custom chr in CGRAM
'°': chr(4), 'g': chr(5), # Custom chr in CGRAM
'ä': chr(225), 'ö': chr(239), 'p': chr(240), # Already available in CGROM
'∑': chr(246), 'Ω': chr(244), 'µ': chr(228)} # todo: investigeate if works if > 127
# Row nr to LCD memory adr for start of row [0–3] [Valid for 4 rows x 20 chars LCD ONLY]
lcd_row_adr = {0: 0x00, 1: 0x40, 2: 0x14, 3: 0x54}
''' First adress LCD kontroller via HA7Scommand "A" '''
dummy = self.tx_rx(b'A' + rom + b'\r', 17) # Adressing
if clear_LCD:
''' Clear display first '''
delay(1)
dummy = self.tx_rx('W0149\r', 3) # Write block '49' 1 byte: Clear LCD
delay(3)
dummy = self.tx_rx('M\r', 17) # Reset AND reselect
line_adr = self.bin2hex(chr(lcd_row_adr[row_nr] + col)) # LCD memory adr to use on LCD for chosen row
''' Convert msg to string of hex bytes
Truncate msg if msg longer than fits on row (20 chars)'''
if use_custom_chars:
s = ''
for char in msg: # Exchange some chars
if char in user_char: # character should be changed?
s += user_char[char] # change char if so
else:
s += char
msg = s
msg_len = len(msg)
if msg_len > (self.ROW_LENGTH - col):
msg_len = self.ROW_LENGTH - col
msg_hex = self.bin2hex(msg[:self.ROW_LENGTH - col]) # Truncate
else:
msg_hex = self.bin2hex(msg) # Convert to hex string (no '0x' before each byte)
''' Can only transfer max 16 chars to scratchpad LCD memory per transfer: First tfr 16 chrs + 2nd tfr for rest '''
if msg_len > 16:
len_hex = self.bin2hex(chr(16 + 2)) # Limit to 16 + 2 bytes first transmission
dummy = self.tx_rx('W' + len_hex + '4E' + line_adr + msg_hex[:16 * 2] + '\r', 37) # Write first 16 chars to
# scratchpad
delay(1)
dummy = self.tx_rx('M\r', 17) # Reset AND reselect
dummy = self.tx_rx('W0148\r', 3) # Copy Scratchpad to LCD
''' Adjust parameters for next part of msg to write to LCD memory '''
msg_len -= 16
msg_hex = msg_hex[16 * 2:] # keep unsent part only
line_adr = self.bin2hex(chr(lcd_row_adr[row_nr] + col + 16)) # LCD memory adr to use on LCD for 17:th
# char
dummy = self.tx_rx('M\r', 17) # Reset AND reselect (enl HA7S doc)
len_hex = self.bin2hex(chr(msg_len + 2)) # Len = BYTE count for remaining data
dummy = self.tx_rx('W' + len_hex + '4E' + line_adr + msg_hex + '\r', len(msg_hex) + 5) # Write to scratchpad
delay(1)
resp1 = self.tx_rx('M\r', 17) # Reset AND reselect
dummy = self.tx_rx('W0148\r', 3) # Copy Scratchpad to LCD
delay(2)
''' Turn LCD back-light ON '''
dummy = self.tx_rx('M\r', 17) # Reset AND reselect
dummy = self.tx_rx('W0108\r', 3) # Write block '08' 1 byte: LCD backlight on
dummy = self.tx_rx('R', 1) # Reset
def tx_rx(self, tx, nr_chars):
""" Send command to and receive respons from SA7S"""
''' rx = uart.readall() # Receive respons TAKES 1.0 sec ALWAYS (after uart.any) TimeOut!!!! '''
i = 0
rx = ''
# todo: do check if respons == same as sent: repeat otherwise
self.uart.write(tx) # Send to unit
# print("uart.write: i, tx: ", i, tx[:-1])
while True: # Typiskt 2–3 (search: 4) varv i loopen
i += 1
if self.uart.any(): # returns True if any characters wait
dbg.high()
strt = micros()
rx = b''
j = 0
while True: # Typically 10–20 (search: 12; M, R & W0144: 1) loops
j += 1
rxb = self.uart.read(nr_chars) # uart.readln och uart.readall ger båda timeout (1s default)
rx = rx + rxb
if (len(rx) >= nr_chars) or (rxb == b'\r'): # End of search returns \r
break
dbg.low()
##print("uart.read: i, j, tx, rx, ∆time ", i, j, tx[:-1], rx, len(rx), elapsed_micros(strt) / 1e6, 's')
delay(84)
break
else:
delay(10)
return rx
def hex_bytes_to_str(self, s):
""" Convert bytes (2 ascii hex char each) to string of ascii chars """
def hex_char_to_int(c):
##print("c:", hex(c), chr(c))
if c >= 65:
c -= 55 # 'A' = 10
else:
c -= 48 # '0' = 0
return c
r = ''
s = str.upper(s) # Raise to upper case
for i in range(0, len(s), 2):
m = hex_char_to_int(s[i])
l = hex_char_to_int(s[i + 1])
t = (m << 4) + (l & 0xf)
r += chr(t)
##print("Byte: r", r)
return r
def hex_byte_to_int(self, s):
""" Convert byte (2 ascii hex char) to int """
def hex_char_to_int(c):
##print("c:", hex(c), chr(c))
if c >= 65:
c -= 55 # 'A' = 10
else:
c -= 48 # '0' = 0
return c
r = []
s = str.upper(s) # Raise to upper case
for i in range(0, len(s), 2):
m = hex_char_to_int(s[i])
l = hex_char_to_int(s[i + 1])
t = (m << 4) + (l & 0xf)
r.append(t)
##print("Byte: m l t r", m, l, t, r)
return r[0]
def lsb_first_hex_ascii_to_int32(self, s):
""" Convert 32 bits hexadecimal ascii-string (LSByte first) to 32 bit integer """
cnt = (self.hex_byte_to_int(s[6:8]) << 24) + (self.hex_byte_to_int(s[4:6]) << 16) + \
(self.hex_byte_to_int(s[2:4]) << 8) + self.hex_byte_to_int(s[0:2])
return cnt
def bin2hex(self, s): # Works for i 0–127; NOT 128+
""" Convert integer to hex string with full width w/o '0x'-prefix """
return binascii.hexlify(s).decode("utf-8")
def int4_to_1hex_string(self, i4):
""" Convert integer to hex string with full width w/o '0x'-prefix """
return ''.join('{:01X}'.format(i4))
def int8_to_2hex_string(self, i8):
""" Convert integer to hex string with full width w/o '0x'-prefix """
return ''.join('{:02X}'.format(i8))
def int16_to_4hex_string(self, i16):
""" Convert integer to hex string with full width w/o '0x'-prefix """
return ''.join('{:04X}'.format(i16))
def int32_to_8hex_string(self, i32):
""" Convert integer to hex string with full width w/o '0x'-prefix """
return ''.join('{:08X}'.format(i32))
bytesToSend = struct.pack('f', deg * 180 / math.pi)
#print("Degrees:" + str(deg * 180 / math.pi) + ", Radius: " + str(r))
#if uart.any() > 0:
#print("DATA INCOMING")
if (len(blobs2[1]) > 0) and (len(blobs2[2]) > 0):
yx = blobs2[1][0].cx()
yy = blobs2[1][0].cy()
bx = blobs2[2][0].cx()
by = blobs2[2][0].cy()
ydeg = math.atan2(yy, yx)
bdeg = math.atan2(by, bx)
yr = math.sqrt(yx**2 + yy**2)
br = math.sqrt(bx**2 + by**2)
print("num" + str((bdeg + ydeg) * 180))
print("b" + str(bdeg * 180))
print("y" + str(ydeg * 180))
while uart.any() != 0:
UARTRecieve = str(uart.read(1), 'utf-8')
#print (UARTRecieve)
if (UARTRecieve == "x"):
UARTRecieve = ''
#print("Codes Match");
for b in bytesToSend:
#uart.write(chr(int(b,2)))
#print(b)
uart.writechar(b)
#feedback-waiting for user to press button orange = LED(3) orange.on() #boolean variable to manage main loop finished = False ######################### # Main Loop # ######################### while finished == False: #if start command is received if hc12.any(): orange.off() #for local use only for i in range(0,2): blue.toggle() sleep(0.4) blue.toggle() sleep(0.4) #X second loop, get data every half second and write to backup.csv, and also transmit to ground station for tag in range(1,61): green.off() temp = bmp180.temperature
class SBUSReceiver:
def __init__(self, uart_port):
self.sbus = UART(uart_port, 100000)
self.sbus.init(100000, bits=8, parity=0, stop=2, timeout_char=3, read_buf_len=250)
# constants
self.START_BYTE = b'0f'
self.END_BYTE = b'00'
self.SBUS_FRAME_LEN = 25
self.SBUS_NUM_CHAN = 18
self.OUT_OF_SYNC_THD = 10
self.SBUS_NUM_CHANNELS = 18
self.SBUS_SIGNAL_OK = 0
self.SBUS_SIGNAL_LOST = 1
self.SBUS_SIGNAL_FAILSAFE = 2
# Stack Variables initialization
self.validSbusFrame = 0
self.lostSbusFrame = 0
self.frameIndex = 0
self.resyncEvent = 0
self.outOfSyncCounter = 0
self.sbusBuff = bytearray(1) # single byte used for sync
self.sbusFrame = bytearray(25) # single SBUS Frame
self.sbusChannels = array.array('H', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # RC Channels
self.isSync = False
self.startByteFound = False
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
def get_rx_channels(self):
"""
Used to retrieve the last SBUS channels values reading
:return: an array of 18 unsigned short elements containing 16 standard channel values + 2 digitals (ch 17 and 18)
"""
return self.sbusChannels
def get_rx_channel(self, num_ch):
"""
Used to retrieve the last SBUS channel value reading for a specific channel
:param: num_ch: the channel which to retrieve the value for
:return: a short value containing
"""
return self.sbusChannels[num_ch]
def get_failsafe_status(self):
"""
Used to retrieve the last FAILSAFE status
:return: a short value containing
"""
return self.failSafeStatus
def get_rx_report(self):
"""
Used to retrieve some stats about the frames decoding
:return: a dictionary containg three information ('Valid Frames','Lost Frames', 'Resync Events')
"""
rep = {}
rep['Valid Frames'] = self.validSbusFrame
rep['Lost Frames'] = self.lostSbusFrame
rep['Resync Events'] = self.resyncEvent
return rep
def decode_frame(self):
# TODO: DoubleCheck if it has to be removed
for i in range(0, self.SBUS_NUM_CHANNELS - 2):
self.sbusChannels[i] = 0
# counters initialization
byte_in_sbus = 1
bit_in_sbus = 0
ch = 0
bit_in_channel = 0
for i in range(0, 175): # TODO Generalization
if self.sbusFrame[byte_in_sbus] & (1 << bit_in_sbus):
self.sbusChannels[ch] |= (1 << bit_in_channel)
bit_in_sbus += 1
bit_in_channel += 1
if bit_in_sbus == 8:
bit_in_sbus = 0
byte_in_sbus += 1
if bit_in_channel == 11:
bit_in_channel = 0
ch += 1
# Decode Digitals Channels
# Digital Channel 1
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 0):
self.sbusChannels[self.SBUS_NUM_CHAN - 2] = 1
else:
self.sbusChannels[self.SBUS_NUM_CHAN - 2] = 0
# Digital Channel 2
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 1):
self.sbusChannels[self.SBUS_NUM_CHAN - 1] = 1
else:
self.sbusChannels[self.SBUS_NUM_CHAN - 1] = 0
# Failsafe
self.failSafeStatus = self.SBUS_SIGNAL_OK
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 2):
self.failSafeStatus = self.SBUS_SIGNAL_LOST
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 3):
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
def get_sync(self):
if self.sbus.any() > 0:
if self.startByteFound:
if self.frameIndex == (self.SBUS_FRAME_LEN - 1):
self.sbus.readinto(self.sbusBuff, 1) # end of frame byte
if self.sbusBuff[0] == 0: # TODO: Change to use constant var value
self.startByteFound = False
self.isSync = True
self.frameIndex = 0
else:
self.sbus.readinto(self.sbusBuff, 1) # keep reading 1 byte until the end of frame
self.frameIndex += 1
else:
self.frameIndex = 0
self.sbus.readinto(self.sbusBuff, 1) # read 1 byte
if self.sbusBuff[0] == 15: # TODO: Change to use constant var value
self.startByteFound = True
self.frameIndex += 1
def get_new_data(self):
"""
This function must be called periodically according to the specific SBUS implementation in order to update
the channels values.
For FrSky the period is 300us.
"""
if self.isSync:
if self.sbus.any() >= self.SBUS_FRAME_LEN:
self.sbus.readinto(self.sbusFrame, self.SBUS_FRAME_LEN) # read the whole frame
if (self.sbusFrame[0] == 15 and self.sbusFrame[
self.SBUS_FRAME_LEN - 1] == 0): # TODO: Change to use constant var value
self.validSbusFrame += 1
self.outOfSyncCounter = 0
self.decode_frame()
else:
self.lostSbusFrame += 1
self.outOfSyncCounter += 1
if self.outOfSyncCounter > self.OUT_OF_SYNC_THD:
self.isSync = False
self.resyncEvent += 1
else:
self.get_sync()
class SBUSReceiver:
def __init__(self):
self.sbus = UART(3, 100000)
self.sbus.init(100000, bits=8, parity=0, stop=2, timeout_char=3, read_buf_len=250)
# constants
self.START_BYTE = b'0f'
self.END_BYTE = b'00'
self.SBUS_FRAME_LEN = 25
self.SBUS_NUM_CHAN = 18
self.OUT_OF_SYNC_THD = 10
self.SBUS_NUM_CHANNELS = 18
self.SBUS_SIGNAL_OK = 0
self.SBUS_SIGNAL_LOST = 1
self.SBUS_SIGNAL_FAILSAFE = 2
# Stack Variables initialization
self.validSbusFrame = 0
self.lostSbusFrame = 0
self.frameIndex = 0
self.resyncEvent = 0
self.outOfSyncCounter = 0
self.sbusBuff = bytearray(1) # single byte used for sync
self.sbusFrame = bytearray(25) # single SBUS Frame
self.sbusChannels = array.array('H', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # RC Channels
self.isSync = False
self.startByteFound = False
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
# logger.info("SBUS Stack Started")
def get_rx_channels(self):
return self.sbusChannels
def get_rx_channel(self, num_ch):
return self.sbusChannels[num_ch]
def get_failsafe_status(self):
return self.failSafeStatus
def get_rx_report(self):
rep = {}
rep['Valid Frames'] = self.validSbusFrame
rep['Lost Frames'] = self.lostSbusFrame
rep['Resync Events'] = self.resyncEvent
return rep
def decode_frame(self):
# TODO: DoubleCheck if it has to be removed
for i in range(0, self.SBUS_NUM_CHANNELS - 2):
self.sbusChannels[i] = 0
# counters initialization
byte_in_sbus = 1
bit_in_sbus = 0
ch = 0
bit_in_channel = 0
for i in range(0, 175): # TODO Generalization
if self.sbusFrame[byte_in_sbus] & (1 << bit_in_sbus):
self.sbusChannels[ch] |= (1 << bit_in_channel)
bit_in_sbus += 1
bit_in_channel += 1
if bit_in_sbus == 8:
bit_in_sbus = 0
byte_in_sbus += 1
if bit_in_channel == 11:
bit_in_channel = 0
ch += 1
# Decode Digitals Channels
# Digital Channel 1
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 0):
self.sbusChannels[self.SBUS_NUM_CHAN - 2] = 1
else:
self.sbusChannels[self.SBUS_NUM_CHAN - 2] = 0
# Digital Channel 2
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 1):
self.sbusChannels[self.SBUS_NUM_CHAN - 1] = 1
else:
self.sbusChannels[self.SBUS_NUM_CHAN - 1] = 0
# Failsafe
self.failSafeStatus = self.SBUS_SIGNAL_OK
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 2):
self.failSafeStatus = self.SBUS_SIGNAL_LOST
if self.sbusFrame[self.SBUS_FRAME_LEN - 2] & (1 << 3):
self.failSafeStatus = self.SBUS_SIGNAL_FAILSAFE
def get_sync(self):
if self.sbus.any() > 0:
if self.startByteFound:
if self.frameIndex == (self.SBUS_FRAME_LEN - 1):
self.sbus.readinto(self.sbusBuff, 1) # end of frame byte
if self.sbusBuff[0] == 0: # TODO: Change to use constant var value
self.startByteFound = False
self.isSync = True
self.frameIndex = 0
else:
self.sbus.readinto(self.sbusBuff, 1) # keep reading 1 byte until the end of frame
self.frameIndex += 1
else:
self.frameIndex = 0
self.sbus.readinto(self.sbusBuff, 1) # read 1 byte
if self.sbusBuff[0] == 15: # TODO: Change to use constant var value
self.startByteFound = True
self.frameIndex += 1
def get_new_data(self):
if self.isSync:
if self.sbus.any() >= self.SBUS_FRAME_LEN:
self.sbus.readinto(self.sbusFrame, self.SBUS_FRAME_LEN) # read the whole frame
if (self.sbusFrame[0] == 15 and self.sbusFrame[
self.SBUS_FRAME_LEN - 1] == 0): # TODO: Change to use constant var value
self.validSbusFrame += 1
self.outOfSyncCounter = 0
self.decode_frame()
else:
self.lostSbusFrame += 1
self.outOfSyncCounter += 1
if self.outOfSyncCounter > self.OUT_OF_SYNC_THD:
self.isSync = False
self.resyncEvent += 1
else:
self.get_sync()
my_gps = MicropyGPS()
def print_out(string):
print(string)
#uart_bt.write(string)
try:
log = open('/sd/log.txt','a')
log.write(string+'\n')
log.close()
except:
print('SD Error')
#uart_bt.write('SD Error\n')
# Continuous Tests for characters available in the UART buffer, any characters are feed into the GPS
# object. When enough char are feed to represent a whole, valid sentence, stat is set as the name of the
# sentence and printed
while True:
pyb.wfi()
if uart_gps.any():
stat = my_gps.update(chr(uart_gps.readchar())) # Note the conversion to to chr, UART outputs ints normally
if stat:
ret = ('--------' + stat + '--------\n')
ret += (my_gps.time_string() + '\n')
ret += (my_gps.latitude_string()+ '\n')
ret += (my_gps.longitude_string()+ '\n')
ret += (my_gps.altitude_string()+ '\n')
ret += (my_gps.speed_string()+ '\n')
print_out(ret)
stat = None
# set up motor with PWM and timer control
A1 = Pin('Y9',Pin.OUT_PP)
A2 = Pin('Y10',Pin.OUT_PP)
pwm_out = Pin('X1')
tim = Timer(2, freq = 1000)
motor = tim.channel(1, Timer.PWM, pin = pwm_out)
# Motor in idle state
A1.high()
A2.high()
speed = 0
DEADZONE = 5
# Use keypad U and D keys to control speed
while True: # loop forever until CTRL-C
while (uart.any()!=10): #wait we get 10 chars
n = uart.any()
command = uart.read(10)
if command[2]==ord('5'):
if speed < 96:
speed = speed + 5
print(speed)
elif command[2]==ord('6'):
if speed > - 96:
speed = speed - 5
print(speed)
if (speed >= DEADZONE): # forward
A1.high()
A2.low()
motor.pulse_width_percent(speed)
elif (speed <= -DEADZONE):
from pyb import UART
import json
#GU620模块初始化
N1 = Pin('Y6', Pin.OUT_PP)#定义通信系统启动引脚
N1.low()
pyb.delay(2000)
N1.high()
pyb.delay(10000)#拉高拉低引脚，启动通信系统
u2 = UART(4,115200,timeout = 50)#定义串口4，设置 波特率为115200
#初始化 HTTP 应用
u2.write('AT+HTTPINIT\r\n')
getURLCMD = 'AT+HTTPPARA=1,"http://old.tpyboard.com/v702/httptest.php?t=123456"\r\n'
#getURLCMD = 'AT+HTTPPARA=1,"https://www.baidu.com"\r\n'
while True:
if u2.any() > 0:
dataRead = u2.read()
print('_dataRead:',dataRead)
print('-'*30)
if dataRead.find(b'OK') > -1:
#AT命令执行成功
#判断是执行的哪一步操作
if dataRead.find(b'AT+HTTPINIT') > -1:
#初始化HTTP成功
#设置 HTTP 参数值 设置url
u2.write(getURLCMD)
elif dataRead.find(b'AT+HTTPPARA=1') > -1:
#HTTP参数设置成功
#发起GET请求获取数据
u2.write('AT+HTTPACTION=0\r\n')
elif dataRead.find(b'AT+HTTPREAD\r\n\r\n+HTTPREAD') > -1:
gps_uart = UART(6, 9600, read_buf_len=1000)
xbee_uart = UART(2, 9600)
Razor_IMU = IMU.Razor(3, 57600)
# Razor_IMU.set_angle_output()
Razor_IMU.set_all_calibrated_output()
# while True:
# a,b,c = Razor_IMU.get_one_frame()
# print(a,b,c)
# Countdown Timer
start = pyb.millis()
backup_timer = 5400000
# Don't do anything until GPS is found
while gps_uart.any() >= 0:
my_gps.update(chr(gps_uart.readchar()))
print("No GPS signal!!!\n")
print(my_gps.latitude)
if my_gps.latitude[0] != 0:
init_lat = convert_latitude(my_gps.latitude)
init_long = convert_longitude(my_gps.longitude)
initial_point = (init_lat, init_long)
print("Initial Point: {}".format(initial_point))
break
# initial_point = (40.870242, -119.106354)
# while True:
# print('not landed yet')
# if pyb.elapsed_millis(start) >= backup_timer: #This is 90 minutes
last_last_bias_down = last_bias_down
last_bias_down = bias_down
return pwm
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size :
max_blob = blob
max_size = blob[2]*blob[3]
return max_blob
while(True):
clock.tick()
while UART.any(UART(3)):
mode_choose = UART.readchar(UART(3))
# mode_choose = 2
# continue
if mode_choose != 1 and mode_choose != 2 and mode_choose != 3:
print("while: %d" % mode_choose)
mode_choose = mode_choose_last
print("real: %d" % mode_choose)
# print("NOW: %d" % mode_choose)
mode_choose_last = mode_choose
if mode_choose == 2:#打气球程序段
# clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([red_threshold])
if blobs:
max_blob = find_max(blobs)
from pyb import UART uart = UART(1) uart = UART(1, 9600) uart = UART(1, 9600, bits=8, stop=1, parity=None) print(uart) uart.init(1200) print(uart) uart.any() uart.send(1, timeout=500)
from pyb import UART
uart = UART(6, 115200) # init with given baudrate
cmdStr = ""
for i in range(1000):
uart.write("hello ")
pyb.delay(100)
if uart.any() > 0:
for i in range(uart.any()):
ch = uart.readchar()
if ch == 0x0d:
print("Command is:", cmdStr)
cmdStr = ""
continue
if ch == 0x0a:
continue
cmdStr += chr(ch)
print (chr(ch))
print(uart0.read(1) == b'1')
print(uart0.read(2) == b'23')
print(uart0.read() == b'')
uart0.write(b'123')
buf = bytearray(3)
print(uart1.readinto(buf, 1) == 1)
print(buf)
print(uart1.readinto(buf) == 2)
print(buf)
# try initializing without the id
uart0 = UART(baudrate=1000000, pins=uart_pins[0][0])
uart0.write(b'1234567890')
pyb.delay(2) # because of the fifo interrupt levels
print(uart1.any() == 10)
print(uart1.readline() == b'1234567890')
print(uart1.any() == 0)
uart0.write(b'1234567890')
print(uart1.readall() == b'1234567890')
# tx only mode
uart0 = UART(0, 1000000, pins=('GP12', None))
print(uart0.write(b'123456') == 6)
print(uart1.read() == b'123456')
print(uart1.write(b'123') == 3)
print(uart0.read() == b'')
# rx only mode
uart0 = UART(0, 1000000, pins=(None, 'GP13'))
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, 4, 5, 6, 7, "Z"):
try:
UART(bus, 9600)
print("UART", bus)
except ValueError:
print("ValueError", bus)
uart = UART(1)
uart = UART(1, 9600)
uart = UART(1, 9600, bits=8, parity=None, stop=1)
print(uart)
uart.init(2400)
print(uart)
print(uart.any())
print(uart.write('123'))
print(uart.write(b'abcd'))
print(uart.writechar(1))
# make sure this method exists
uart.sendbreak()
# non-blocking mode
uart = UART(1, 9600, timeout=0)
print(uart.write(b'1'))
print(uart.write(b'abcd'))
print(uart.writechar(1))
print(uart.read(100))
orange.off()
#create switch object
big_red_button = Switch()
big_red_button.callback(start)
finished = False
#########################
# Main Loop #
#########################
while finished == False: #While loop that loops forever
if hc12.any():
data = hc12.readline()
data = data.decode('utf-8')
dataArray = data.split(',') #Split it into an array called dataArray
if dataArray[0] == 'end':
green.off()
sleep(0.5)
green.on()
sleep(0.5)
green.off()
finished == True
elif len(dataArray) == 6:
tagx = dataArray[0]
temp = dataArray[1]
