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)
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 Wifi_Driver(object):
def __init__(self, uart_num=2, uart_baud=9600):
self.uart = UART(uart_num, uart_baud)
self.uart.init(uart_baud, bits=8, parity=None, stop=1)
def receive_char(self):
temp = self.uart.readchar()
if temp == -1:
return None
temp = chr(temp)
return temp
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 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 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
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
pres = bmp180.pressure
alt = bmp180.altitude
#if there is gps data to be read
while uart.any():
my_sentence = chr(uart.readchar())
for x in my_sentence:
my_gps.update(x)
latitude = my_gps.latitude_string()
longitude = my_gps.longitude_string()
timestamp = my_gps.timestamp
#alt2 = my_gps.altitude
#open backup.csv to write data to, write to it, then close it
backup = open('/sd/backup.csv', 'a')
backup.write('{},{},{},{},{},{},{}\n'.format(tag,timestamp,temp,pres,alt,latitude,longitude))
backup.close()
data = str(tag) + ',' + str(temp) + ',' + str(pres) + ',' + str(alt) + ',' + str(latitude) + ',' + str(longitude) #concatenate data with commas
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)
down_error_ori = max_blob.cx()-img.width()/2
#color_b=statistics.b_mode()
#print("center area:")
#print(color_l,color_a,color_b)
#print(colorMiddle_a)
img.draw_rectangle(ROI_M)
statistics=img.get_statistics(roi=ROI_R)
#color_l=statistics.l_mode()
colorRight_a=statistics.a_mode()
#color_b=statistics.b_mode()
#print("right area:")
#print(color_l,color_a,color_b)
#print(colorRight_a)
img.draw_rectangle(ROI_R)
while(True):
if(uart.readchar()==83):
LED(2).off()
LED(1).on()
time.sleep(1000)
print('ok')
img = sensor.snapshot() # Take a picture and return the image.
#print('waiting for host')
#print(uart.readchar())
statistics=img.get_statistics(roi=ROI_L)
#color_l=statistics.l_mode()
colorLeft_a=statistics.a_mode()
#color_b=statistics.b_mode()
#print("left area:")
#print(color_l,color_a,color_b)
#print(colorLeft_a)
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:
increase = False
class SPO2:
def __init__(self):
self.uart6 = UART(6, baudrate=4800, bits=8, parity=1, stop=1)
def fill(self, sData):
l = len(sData)
lData = '0' * (8 - l) + sData
return lData
def binToInt(self, bData):
iData = 0
for i in range(len(bData)):
if (bData[-(i + 1)] == '1'):
iData = iData + 2**i
return iData
def receiveData(self):
while True:
byte1 = self.uart6.readchar()
if byte1 >= 128:
byte2 = self.uart6.readchar()
byte3 = self.uart6.readchar()
byte4 = self.uart6.readchar()
byte5 = self.uart6.readchar()
if byte2 < 128 and byte3 < 128 and byte4 < 128 and byte5 < 128:
dataList = ['', '', '', '', '']
dataList[0] = self.fill(bin(byte1)[2:])
dataList[1] = self.fill(bin(byte2)[2:])
dataList[2] = self.fill(bin(byte3)[2:])
dataList[3] = self.fill(bin(byte4)[2:])
dataList[4] = self.fill(bin(byte5)[2:])
return dataList
def getSpList(self):
dataList = self.receiveData()
spList = []
if (dataList[0][-7] == '1'):
print("Pulse rate sound is on")
if (dataList[0][-5] == '1'):
print("Too long time in searching")
elif (dataList[0][-6] == '1'):
print("Oxygen saturation is getting lower")
elif (dataList[2][-5] == '1'):
print("Probe has something wrong")
elif (dataList[2][-6] == '1'):
print("Detecting pulse rate")
else:
signalStrength = self.binToInt(dataList[0][-4:])
volumeGraph = self.binToInt(dataList[1][-7:])
barGraph = self.binToInt(dataList[2][-4:])
pulseRate = self.binToInt(dataList[2][-7] + dataList[3][-7:])
spO2 = self.binToInt(dataList[4][-7:])
spList.append(signalStrength)
spList.append(volumeGraph)
spList.append(barGraph)
spList.append(pulseRate)
spList.append(spO2)
return spList
uart.write("\n")
time.sleep(5)
else:
if gap_available > 0:
if len(pos) == 0:
print(len(pos), -99, sep=", ")
else:
print(len(pos), pos, sep=", ")
else:
print(len(pos), 99, sep=", ")
#Read serial if anything available
if Serial == 1:
if uart.any() > 0:
setMode = uart.readchar()
setLighting(setMode)
# modify image in case you want to suppose actual image analzed
#img.blend("bg.bmp", alpha=127)
if WiFi == 1:
cimage = img.compressed(quality=20)
client.send("\r\n--openmv\r\n" \
"Content-Type: image/jpeg\r\n" \
"Content-Length:"+str(cimage.size())+"\r\n\r\n")
client.send(cimage)
#print(clock.fps())
#img = sensor.snapshot() # Take a picture and return the image.
from pyb import UART
from micropyGPS import MicropyGPS
uart = UART(3, 9600)
my_gps = MicropyGPS()
# Reads 300 sentences and reports how many were parsed and if any failed the CRC check
sentence_count = 0
while True:
if uart.any():
stat = my_gps.update(chr(uart.readchar()))
if stat:
print(stat)
stat = None
sentence_count += 1
if sentence_count == 300:
break;
print('Sentences Found:', my_gps.clean_sentences)
print('Sentences Parsed:', my_gps.parsed_sentences)
print('CRC_Fails:', my_gps.crc_fails)
finishROIS = [(0, 90, 160, 10)]
roiX = 1
c = '0'
xIndex = 0
xdiff = 0
errorBuff = [0, 0, 0, 0]
widthBuff = [0, 0, 0, 0]
brakeFlag = 0
noBrake = 0
straightCount = 0
forward()
while (1):
if (uart.any()):
c = uart.readchar() # save characters
ch2.pulse_width(int(0))
while (1):
#if(uart.any()): #check if UART has characters
#c = uart.readchar() # save characters
#print(c)
clock.tick() # Update the FPS clock.
img = sensor.snapshot() # Take a picture and return the image.
#for rfinish in finishROIS :
#finishblobs = img.find_blobs([thresholds], roi=rfinish, pixels_threshold=100, area_threshold=100, merge=True)
#if len(finishblobs) == 3:
#while(1):
#green=[(36, 58, -39, -24, -3, 19)]#green
#red=[(41, 54, 67, 80, 30, 63)] # generic_blue_thresholds,green(36, 58, -39, -24, -3, 19),
#blue (41, 54, 57, 80, 16, 63)
#sensor.reset()
#sensor.set_pixformat(sensor.RGB565)
#sensor.set_framesize(sensor.QQVGA)
#sensor.skip_frames(time = 2000)
#sensor.set_auto_gain(False) # must be turned off for color tracking
#sensor.set_auto_whitebal(False) # must be turned off for color tracking
#sensor.set_vflip(False)
#sensor.set_hmirror(False)
#clock = time.clock()
uart=UART(3,19200)
#track_color()
color_track()
while True:
key=uart.readchar()
if key==1:
cal()
elif key==2:
color_track()
elif key==3:
print(3)
elif key==4:
print(4)
from pyb import UART
# 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
uart = UART(3, 9600)
# Basic UART --> terminal printer, use to test your GPS module
while True:
if uart.any():
print(chr(uart.readchar()), end='')
import sensor, image, time
from pyb import UART
#Sensor Setup
#################################
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
#################################
#Communication Setup
#################################
uart = UART(1, 9600, timeout_char=1000) # init with given baudrate
uart.init(9600, bits=8, parity=None, stop=1, timeout_char=1000) # init with given parameters
#################################
#Main Loop
#################################
clock = time.clock()
while(True):
clock.tick()
while(uart.any() ==0):
continue
#print(20)
while(uart.any() !=0):
print("The returned value is: " ,uart.readchar())
time.sleep(1000)
uart.writechar(8)
#################################
from pyb import UART
uart = UART(1, 115200, bits=8, parity=None, stop=1)
uart.writechar(0x55)
uart.writechar(0xaa)
uart.writechar(0x55)
uart.writechar(0xaa)
uart.write('\r\n')
uart.write('>>')
read_char = 0
while (read_char != 113): #exit if received 'q'
if (uart.any() != 0):
read_char = uart.readchar()
print(read_char)
uart.writechar(read_char)
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
pres = bmp180.pressure
alt = bmp180.altitude
#if there is gps data to be read
while uart.any():
my_sentence = chr(uart.readchar())
for x in my_sentence:
my_gps.update(x)
latitude = my_gps.latitude_string()
longitude = my_gps.longitude_string()
timestamp = my_gps.timestamp
#alt2 = my_gps.altitude
#open backup.csv to write data to, write to it, then close it
backup = open('/sd/backup.csv', 'a')
backup.write('{},{},{},{},{},{},{}\n'.format(
tag, timestamp, temp, pres, alt, latitude, longitude))
backup.close()
pwm = 1600
motor = max_speed - 50
elif (char == 56 or char == 107):
pwm = 1700
motor = max_speed - 75
elif (char == 57 or char == 108): #To the Right
pwm = 1800
motor = max_speed - 100
while (True):
#Prints to UART and Terminal
clock.tick() # Track elapsed milliseconds between snapshots().
if (uart.any()): #Checks for UART
char = uart.readchar() #Grab Bluetooth Command
if (char == 113): #Press Q to turn ON Auto Mode
STATE = ON
forward()
if (char == 101): #Press E to turn OFF Car
STATE = OFF
if (char == 98): #Press B to turn to Bluetooth Mode
STATE = BLUETOOTH
#State Machine
if (STATE == OFF): #Turn Car OFF
led1.on() #Turn Red ON
led2.off()
#Gps Testing to get NMEA Data
#PyBoard
#Created by: Joseph Fuentes [08/09/2017]
import pyb
from pyb import UART
import micropyGPS
#Use GPS on UART 6
#Connect RX of GPS to 'Y1', TX to 'Y2'
uart = UART(6, 9600) # init with given baudrate
uart.init(9600, bits=8, parity=None, stop=1) # init with given parameters
ultimate_GPS= MicropyGPS()
while uart.any()
ultimate_gps.update(chr(uart.readchar()))
pres_lat = convert_latitude(ultimate_gps.latitude)
pres_long = convert_longitude(ultimate_gps.longitude)
present_point = (pres_lat, pres_long)
print(present_point)
#counterUart = bufferUartLength
line5Len = 16 - len(radTextSum) - len(readTextAdd)
whspcL5 = ' ' * line5Len
uart1.readinto(buf, 3)
Line5text = radTextSum + readTextAdd + whspcL5 #+ chr(buf[0])
#+ str(mfreq)
textArray_axis_x = Line1text + LineBreak + Line2text + Line3text + whiteSpaceL3 + Line5text #+ str(buf[0])
# function for formating text: l1 = , L2 = , ...
textSpace = displayText(textArray_axis_x, 0)
c_ar = bytearray(c)
b = textSpace + c_ar
#################### SENDING BUFFER ################################
buffer = bytearray(b)
if uart1.any() > 0:
rec_data = uart1.readchar()
else:
rec_data = 'a'
for cmd in display_cmds:
write_command(cmd)
dc.high()
spi.send(buffer)
#can1.send('a', 257)
pyb.delay(10)
import sensor, image, time
from pyb import UART
#Sensor Setup
#################################
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
#################################
#Communication Setup
#################################
uart = UART(1, 9600, timeout_char=1000) # init with given baudrate
uart.init(9600, bits=8, parity=None, stop=1,
timeout_char=1000) # init with given parameters
#################################
#Main Loop
#################################
clock = time.clock()
while (True):
clock.tick()
while (uart.any() == 0):
continue
print(20)
while (uart.any() != 0):
print(uart.readchar())
time.sleep(1000)
uart.writechar(8)
#################################
# micropyGPS Sentence Test
# When properly connected to working GPS module,
# will print the names of the sentences it receives
# If you are having issues receiving sentences, use UART_test.py to ensure
# your UART is hooked up and configured correctly
from pyb import UART
from micropyGPS import 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
uart = UART(3, 9600)
# Instatntiate the micropyGPS object
my_gps = MicropyGPS()
# 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:
if uart.any():
stat = my_gps.update(chr(uart.readchar())) # Note the conversion to to chr, UART outputs ints normally
if stat:
print(stat)
stat = None
if blobs[i].pixels() > most_pixels:
most_pixels = blobs[i].pixels()
largest_blob = i
#位置环用到的变量
[x, y, w, h] = blobs[largest_blob].rect()
err_x = int(120 - blobs[largest_blob].cy())
err_y = int(blobs[largest_blob].cx() - 160)
img.draw_cross(blobs[largest_blob].cx(),
blobs[largest_blob].cy()) #调试使用
img.draw_rectangle(blobs[largest_blob].rect())
#lcd.display(img)
else:
err_x = 0
err_y = 0
if (uart.any()): #判断是否有串口数据
uart_data = uart.readchar()
if uart_data == 0XAA:
err_min = err_min + 1
elif uart_data == 0XBB:
err_max = err_min - 1
elif uart_data == 0XCC:
err_max = err_max + 1
elif uart_data == 0XDD:
err_max = err_max - 1
if err_x > err_min and err_x < err_max:
red_led.on()
blue_led.off()
pin0.value(0)
pin1.value(0)
elif err_x < err_min:
red_led.off()
if (abs(SteerDiff) > BrakeThreshold) and (cross == False) and (brakeCounter > 60):
uart.write('braking')
ch3 = timer2.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P6"), pulse_width_percent = 0)
brakeCounter = 0
INA.low()
INB.high()
pyb.delay(int(BrakeTime * DutyCycle * 0.017)) # Delay in ms, proportional to duty cycle
INA.high()
INB.low()
# Some braking parameters
cross = False
######## BLUETOOTH #########
if (uart.any() != 0): # if there are characters to be read
p = uart.readchar() # read first character
print(p)
if (p == ord('s')): # Stop, ascii: 115
stop = True
print("stop = ", stop)
if (p == ord('r')): # Restart, ascii: 114
stop = False
print("stop = ", stop)
red_led.off()
green_led.off()
blue_led.on()
if (p == ord('p')): # Kp value, ascii: 112
# Read ascii values and convert to integer.
n1 = uart.readchar() - 48
n2 = uart.readchar() - 48
Kp1 = (n1 * 10) + n2
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
# break
else:
err_x = int(60 - y - h / 2)
err_y = int(x - 80)
#画跟踪线
img.draw_line([int(err_y + 80), int(60 - err_x), int(err_y + 80), 60])
img.draw_line([int(err_y + 80), int(60 - err_x), 80, int(60 - err_x)])
img.draw_cross(int(x + w / 2), int(y + h / 2))
img.draw_rectangle([x, y, w, h])
else:
err_x = 0
err_y = 0
m = m + 1 #数据发送频率
num = uart.any() #读取串口接收到的字符数
#串口接收超声波数据
if (num == 2):
uart_rebuf[0] = uart.readchar()
uart_rebuf[1] = uart.readchar()
rebuf_temp = uart_rebuf[0] = int(uart_rebuf[0] << 8 | uart_rebuf[1])
if (rebuf_temp <= 4000):
high = rebuf_temp
if (num > 2):
error_sta = error_sta + 1
for i in range(num):
uart.readchar()
#数组中数据写入
uart_buf = bytearray([
0x55, 0xAA, 0x10, 0x01, 0x00, high >> 8, high, err_x >> 8, err_x,
err_y >> 8, err_y, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xAA, 0x0D, 0x0a
])
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
# break
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))
# 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 Lora():
LoraErrors = {
"": LoraErrorTimeout, # empty buffer
"+ERR": LoraError,
"+ERR_PARAM": LoraErrorParam,
"+ERR_BUSY": LoraErrorBusy,
"+ERR_PARAM_OVERFLOW": LoraErrorOverflow,
"+ERR_NO_NETWORK": LoraErrorNoNetwork,
"+ERR_RX": LoraErrorRX,
"+ERR_UNKNOWN": LoraErrorUnknown
}
def __init__(self, uart=None, rst_pin=None, boot_pin=None, band=BAND_EU868, poll_ms=300000, debug=False):
self.debug = debug
self.uart = uart
self.rst_pin = rst_pin
self.boot_pin = boot_pin
self.band = band
self.poll_ms = poll_ms
self.last_poll_ms = ticks_ms()
self.init_modem()
# Reset module
self.boot_pin.value(0)
self.rst_pin.value(1)
sleep_ms(200)
self.rst_pin.value(0)
sleep_ms(200)
self.rst_pin.value(1)
# Restart module
self.restart()
def init_modem(self):
# Portenta vision shield configuration
if not self.rst_pin:
self.rst_pin = Pin('PC6', Pin.OUT_PP, Pin.PULL_UP, value=1)
if not self.boot_pin:
self.boot_pin = Pin('PG7', Pin.OUT_PP, Pin.PULL_DOWN, value=0)
if not self.uart:
self.uart = UART(8, 19200)
#self.uart = UART(1, 19200) # Use external module
self.uart.init(19200, bits=8, parity=None, stop=2, timeout=250, timeout_char=100)
def debug_print(self, data):
if self.debug:
print(data)
def is_arduino_firmware(self):
return 'ARD-078' in self.fw_version
def configure_class(self, _class):
self.send_command("+CLASS=", _class)
def configure_band(self, band):
self.send_command("+BAND=", band)
if (band == BAND_EU868 and self.is_arduino_firmware()):
self.send_command("+DUTYCYCLE=", 1)
return True
def set_baudrate(self, baudrate):
self.send_command("+UART=", baudrate)
def set_autobaud(self, timeout=10000):
start = ticks_ms()
while ((ticks_ms() - start) < timeout):
if (self.send_command('', timeout=200, raise_error=False) == '+OK'):
sleep_ms(200)
while (self.uart.any()):
self.uart.readchar()
return True
return False
def get_fw_version(self):
dev = self.send_command("+DEV?")
fw_ver = self.send_command("+VER?")
return dev + " " + fw_ver
def get_device_eui(self):
return self.send_command("+DEVEUI?")
def factory_default(self):
self.send_command("+FACNEW")
def restart(self):
if (self.set_autobaud() == False):
raise(LoraError("Failed to set autobaud"))
# Different delimiter as REBOOT response EVENT doesn't end with '\r'.
if (self.send_command("+REBOOT", delimiter="+EVENT=0,0", timeout=10000) != "+EVENT=0,0"):
raise(LoraError("Failed to reboot module"))
sleep_ms(1000)
self.fw_version = self.get_fw_version()
self.configure_band(self.band)
def set_rf_power(self, mode, power):
self.send_command("+RFPOWER=", mode, ",", power)
def set_port(self, port):
self.send_command("+PORT=", port)
def set_public_network(self, enable):
self.send_command("+NWK=", int(enable))
def sleep(self, enable):
self.send_command("+SLEEP=", int(enable))
def format(self, hexMode):
self.send_command("+DFORMAT=", int(hexMode))
def set_datarate(self, dr):
self.send_command("+DR=", dr)
def get_datarate(self):
return int(self.send_command("+DR?"))
def set_adr(self, adr):
self.send_command("+ADR=", int(adr))
def get_adr(self):
return int(self.send_command("+ADR?"))
def get_devaddr(self):
return self.send_command("+DEVADDR?")
def get_nwk_skey(self):
return self.send_command("+NWKSKEY?")
def get_appskey(self):
return self.send_command("+APPSKEY?")
def get_rx2dr(self):
return int(self.send_command("+RX2DR?"))
def set_rx2dr(self, dr):
self.send_command("+RX2DR=", dr)
def get_ex2freq(self):
return int(self.send_command("+RX2FQ?"))
def set_rx2freq(self, freq):
self.send_command("+RX2FQ=", freq)
def set_fcu(self, fcu):
self.send_command("+FCU=", fcu)
def get_fcu(self):
return int(self.send_command("+FCU?"))
def set_fcd(self, fcd):
self.send_command("+FCD=", fcd)
def get_fcd(self):
return int(self.send_command("+FCD?"))
def change_mode(self, mode):
self.send_command("+MODE=", mode)
def join(self, timeout_ms):
if self.send_command("+JOIN", timeout=timeout_ms) != "+ACK":
return False
response = self.receive('\r', timeout=timeout_ms)
return response == "+EVENT=1,1"
def get_join_status(self):
return int(self.send_command("+NJS?")) == 1
def get_max_size(self):
if (self.is_arduino_firmware()):
return 64
return int(self.send_command("+MSIZE?", timeout=2000))
def poll(self):
if ((ticks_ms() - self.last_poll_ms) > self.poll_ms):
self.last_poll_ms = ticks_ms()
# Triggers a fake write
self.send_data('\0', True)
def send_data(self, buff, confirmed=True):
max_len = self.get_max_size()
if (len(buff) > max_len):
raise(LoraError("Packet exceeds max length"))
if self.send_command("+CTX " if confirmed else "+UTX ", len(buff), data=buff) != "+OK":
return False
if confirmed:
response = self.receive('\r', timeout=10000)
return response == "+ACK"
return True
def receive_data(self, timeout=1000):
response = self.receive('\r', timeout=timeout)
if response.startswith("+RECV"):
params = response.split("=")[1].split(",")
port = params[0]
length = int(params[1])
dummy_data_length = 2 # Data starts with \n\n sequence
data = self.receive(max_bytes=length+dummy_data_length, timeout=timeout)[dummy_data_length:]
return {'port' : port, 'data' : data}
def receive(self, delimiter=None, max_bytes=None, timeout=1000):
buf = []
start = ticks_ms()
while ((ticks_ms() - start) < timeout):
while (self.uart.any()):
buf += chr(self.uart.readchar())
if max_bytes and len(buf) == max_bytes:
data = ''.join(buf)
self.debug_print(data)
return data
if (len(buf) and delimiter != None):
data = ''.join(buf)
trimmed = data[0:-1] if data[-1] == '\r' else data
if (isinstance(delimiter, str) and len(delimiter) == 1 and buf[-1] == delimiter):
self.debug_print(trimmed)
return trimmed
if (isinstance(delimiter, str) and trimmed == delimiter):
self.debug_print(trimmed)
return trimmed
if (isinstance(delimiter, list) and trimmed in delimiter):
self.debug_print(trimmed)
return trimmed
data = ''.join(buf)
self.debug_print(data)
return data[0:-1] if len(data) != 0 and data[-1] == '\r' else data
def available(self):
return self.uart.any()
def join_OTAA(self, appEui, appKey, devEui=None, timeout=60000):
self.change_mode(MODE_OTAA)
self.send_command("+APPEUI=", appEui)
self.send_command("+APPKEY=", appKey)
if (devEui):
self.send_command("+DEVEUI=", devEui)
network_joined = self.join(timeout)
# This delay was in MKRWAN.h
#delay(1000);
return network_joined
def join_ABP(self, nwkId, devAddr, nwkSKey, appSKey, timeout=60000):
self.change_mode(MODE_ABP)
# Commented in MKRWAN.h
#self.send_command("+IDNWK=", nwkId)
self.send_command("+DEVADDR=", devAddr)
self.send_command("+NWKSKEY=", nwkSKey)
self.send_command("+APPSKEY=", appSKey)
self.join(timeout)
return self.get_join_status()
def handle_error(self, command, data):
if not data.startswith("+ERR") and data != "":
return
if (data in self.LoraErrors):
raise(self.LoraErrors[data]('Command "%s" has failed!'%command))
raise(LoraError('Command: "%s" failed with unknown status: "%s"'%(command, data)))
def send_command(self, cmd, *args, delimiter='\r', data=None, timeout=1000, raise_error=True):
# Write command and args
uart_cmd = 'AT'+cmd+''.join([str(x) for x in args])+'\r'
self.debug_print(uart_cmd)
self.uart.write(uart_cmd)
# Attach raw data
if data:
self.debug_print(data)
self.uart.write(data)
# Read status and value (if any)
response = self.receive(delimiter, timeout=timeout)
# Error handling
if raise_error:
self.handle_error(cmd, response)
if cmd.endswith('?'):
return response.split('=')[1]
return response
# 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
pyb.delay(10000) # change to 10000 for real launch
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("")
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))
FACE_TIMEOUT = 500
X_MAG = 80
Y_MAG = 80
c = pyb.millis()
saved = []
while (True):
img = sensor.snapshot() # Take a picture and return the image.
blobs = list(
img.find_features(front_face, threshold=0.95, scale_factor=1.25))
invalidated = (pyb.millis() - c > FACE_TIMEOUT) or \
len(blobs) != len(saved) or \
any([abs(f[0] - l[0]) > X_MAG or abs(f[1] - l[1]) > Y_MAG for f, l in zip(blobs, saved)])
if invalidated:
c = pyb.millis()
if pi.any() and pi.readchar() == 55:
print("Request Received")
cimg = img.compress()
valid = 11 if invalidated and blobs else 00
print("Sending image. Detect: {}".format(valid))
pi.writechar(valid)
pi.write(pack('>i', cimg.size()))
pi.write(cimg)
saved = list(blobs)
