HW15 - ndm736/ME433_2023 GitHub Wiki
The following code reads the camera and displays the image, and prints the frame rate to the computer:
# requires adafruit_ov7670.mpy and adafruit_st7735r.mpy in the lib folder
import time
from displayio import (
Bitmap,
Group,
TileGrid,
FourWire,
release_displays,
ColorConverter,
Colorspace,
)
from adafruit_st7735r import ST7735R
import board
import busio
import digitalio
from adafruit_ov7670 import (
OV7670,
OV7670_SIZE_DIV1,
OV7670_SIZE_DIV8,
OV7670_SIZE_DIV16,
)
release_displays()
spi = busio.SPI(clock=board.GP2, MOSI=board.GP3)
display_bus = FourWire(spi, command=board.GP0, chip_select=board.GP1, reset=None)
display = ST7735R(display_bus, width=160, height=128, rotation=90)
# Ensure the camera is shut down, so that it releases the SDA/SCL lines,
# then create the configuration I2C bus
with digitalio.DigitalInOut(board.GP10) as reset:
reset.switch_to_output(False)
time.sleep(0.001)
bus = busio.I2C(board.GP9, board.GP8) #GP9 is SCL, GP8 is SDA
# Set up the camera (you must customize this for your board!)
cam = OV7670(
bus,
data_pins=[
board.GP12,
board.GP13,
board.GP14,
board.GP15,
board.GP16,
board.GP17,
board.GP18,
board.GP19,
], # [16] [org] etc
clock=board.GP11, # [15] [blk]
vsync=board.GP7, # [10] [brn]
href=board.GP21, # [27/o14] [red]
mclk=board.GP20, # [16/o15]
shutdown=None,
reset=board.GP10,
) # [14]
width = display.width
height = display.height
bitmap = None
# Select the biggest size for which we can allocate a bitmap successfully, and
# which is not bigger than the display
for size in range(OV7670_SIZE_DIV1, OV7670_SIZE_DIV16 + 1):
#cam.size = size # for 4Hz
#cam.size = OV7670_SIZE_DIV16 # for 30x40, 9Hz
cam.size = OV7670_SIZE_DIV8 # for 60x80, 9Hz
if cam.width > width:
continue
if cam.height > height:
continue
try:
bitmap = Bitmap(cam.width, cam.height, 65536)
break
except MemoryError:
continue
print(width, height, cam.width, cam.height)
if bitmap is None:
raise SystemExit("Could not allocate a bitmap")
time.sleep(4)
g = Group(scale=1, x=(width - cam.width) // 2, y=(height - cam.height) // 2)
tg = TileGrid(
bitmap, pixel_shader=ColorConverter(input_colorspace=Colorspace.BGR565_SWAPPED)
)
g.append(tg)
display.show(g)
t0 = time.monotonic_ns()
display.auto_refresh = False
while True:
cam.capture(bitmap)
bitmap.dirty()
display.refresh(minimum_frames_per_second=0)
t1 = time.monotonic_ns()
print("fps", 1e9 / (t1 - t0))
t0 = t1
The following code waits for a newline from the computer, then prints the raw pixel value to the computer for a row in the bitmap:
# requires adafruit_ov7670.mpy and adafruit_st7735r.mpy in the lib folder
import time
from displayio import (
Bitmap,
Group,
TileGrid,
FourWire,
release_displays,
ColorConverter,
Colorspace,
Palette,
)
from adafruit_st7735r import ST7735R
import board
import busio
import digitalio
from adafruit_ov7670 import (
OV7670,
OV7670_SIZE_DIV1,
OV7670_SIZE_DIV8,
OV7670_SIZE_DIV16,
)
from ulab import numpy as np
release_displays()
spi = busio.SPI(clock=board.GP2, MOSI=board.GP3)
display_bus = FourWire(spi, command=board.GP0, chip_select=board.GP1, reset=None)
display = ST7735R(display_bus, width=160, height=128, rotation=90)
# Ensure the camera is shut down, so that it releases the SDA/SCL lines,
# then create the configuration I2C bus
with digitalio.DigitalInOut(board.GP10) as reset:
reset.switch_to_output(False)
time.sleep(0.001)
bus = busio.I2C(board.GP9, board.GP8) #GP9 is SCL, GP8 is SDA
# Set up the camera (you must customize this for your board!)
cam = OV7670(
bus,
data_pins=[
board.GP12,
board.GP13,
board.GP14,
board.GP15,
board.GP16,
board.GP17,
board.GP18,
board.GP19,
], # [16] [org] etc
clock=board.GP11, # [15] [blk]
vsync=board.GP7, # [10] [brn]
href=board.GP21, # [27/o14] [red]
mclk=board.GP20, # [16/o15]
shutdown=None,
reset=board.GP10,
) # [14]
width = display.width
height = display.height
bitmap = None
# Select the biggest size for which we can allocate a bitmap successfully, and
# which is not bigger than the display
for size in range(OV7670_SIZE_DIV1, OV7670_SIZE_DIV16 + 1):
#cam.size = size # for 4Hz
#cam.size = OV7670_SIZE_DIV16 # for 30x40, 9Hz
cam.size = OV7670_SIZE_DIV8 # for 60x80, 9Hz
if cam.width > width:
continue
if cam.height > height:
continue
try:
bitmap = Bitmap(cam.width, cam.height, 65536)
break
except MemoryError:
continue
print(width, height, cam.width, cam.height)
if bitmap is None:
raise SystemExit("Could not allocate a bitmap")
time.sleep(4)
g = Group(scale=1, x=(width - cam.width) // 2, y=(height - cam.height) // 2)
tg = TileGrid(
bitmap, pixel_shader=ColorConverter(input_colorspace=Colorspace.BGR565_SWAPPED)
)
g.append(tg)
display.show(g)
t0 = time.monotonic_ns()
display.auto_refresh = False
# arrays to store the color data
reds = np.zeros(60,dtype=np.uint16)
greens = np.zeros(60,dtype=np.uint16)
blues = np.zeros(60,dtype=np.uint16)
bright = np.zeros(60)
while True:
cam.capture(bitmap)
#bitmap[10,10] = 0 # set a pixel to black
#bitmap[10,20] = 0 # [Y,X], [0,0] is bottom left
# colors:
#0x1F -> 0b0000000000011111 # all green
#0x7E0 -> 0b0000011111100000 # all red
#0xF800 -> 0b1111100000000000 # all blue
# wait for a newline from the computer
input()
row = 40 # which row to send to the computer
# draw a red dot above the row, in the middle
bitmap[row+1,30] = 0x3F<<5
# force some colors to test the bits
#for i in range(0,20):
# bitmap[row,i] = 0xF800 # blue
#for i in range(20,40):
# bitmap[row,i] = 0x1F # green
#for i in range(40,60):
# bitmap[row,i] = 0x7E0 # red
# calculate the colors
for i in range(0,60):
reds[i] = ((bitmap[row,i]>>5)&0x3F)/0x3F*100
greens[i] = ((bitmap[row,i])&0x1F)/0x1F*100
blues[i] = (bitmap[row,i]>>11)/0x1F*100
bright[i] = reds[i]+greens[i]+blues[i]
# threshold to try to find the line
#for i in range(0,60):
# if (reds[i]>50 and blues[i]>50):
# bitmap[row,i] = 0xFFFF
# else:
# bitmap[row,i] = 0x0000
# print the raw pixel value to the computer
for i in range(0,60):
print(str(i)+" "+str(bitmap[row,i]))
bitmap.dirty() # updae the image on the screen
display.refresh(minimum_frames_per_second=0)
t1 = time.monotonic_ns()
#print("fps", 1e9 / (t1 - t0))
t0 = t1
Computer code to read a line of colors from a bitmap and plot:
# get a line of raw bitmap and plot the components
import serial
ser = serial.Serial('COM14',230400) # the name of your Pico port
print('Opening port: ')
print(ser.name)
ser.write(b'hi\r\n') # send a newline to request data
data_read = ser.read_until(b'\n',50) # read the echo
sampnum = 0
index = 0
raw = []
reds = []
greens = []
blues = []
bright = []
# Pico sends back index and raw pixel value
while sampnum < 60: # width of bitmap
data_read = ser.read_until(b'\n',50) # read until newline
data_text = str(data_read,'utf-8') # convert bytes to string
data = list(map(int,data_text.split())) # convert string to values
if(len(data)==2):
index = data[0]
raw.append(data[1])
reds.append(((data[1]>>5)&0x3F)/0x3F*100) # red value is middle 6 bits
greens.append((data[1]&0x1F)/0x1F*100) # green value is rightmost 5 bits
blues.append(((data[1]>>11)&0x1F)/0x1F*100) # blue vale is leftmost 5 bits
bright.append((data[1]&0x1F)+((data[1]>>5)&0x3F)+((data[1]>>11)&0x1F)) # sum of colors
sampnum = sampnum + 1
# print the raw color as a 16bit binary to double check bitshifting
for i in range(len(reds)):
print(f"{raw[i]:#018b}")
# plot the colors
import matplotlib.pyplot as plt
x = range(len(reds)) # time array
plt.plot(x,reds,'r*-',x,greens,'g*-',x,blues,'b*-')
plt.ylabel('color')
plt.xlabel('position')
plt.show()
# be sure to close the port
ser.close()
When you are able to see the plots of the amount of red, green, and blue, the challenge becomes writing an algorithm to identify the center of the line.
One technique is to threshold the pixels and assigning them to white or black, or white/black/green/red/blue. Then perform a center of mass calculation (COM = sum(color x index) / sum(color)). Print the COM and test for a variety of lighting and color conditions.
When you have a COM, print it over the UART to the PIC:
# Pico UART code
# in your init area
uart = busio.UART(board.GP4, board.GP5, baudrate=9600) #tx pin, rx pin
# in while True: after reading an image and finding the line
value_str = str(value)+'\n'
uart.write(value_str.encode())
On the PIC, enable UART2 to read the COM:
// PIC UART2 code
#include "uart2.h" // include the library
// after NU32DIP_startup()
UART2_Startup();
// in while(1)
int com = 0;
// uart2_flag() is 1 when uart2 has rx a message and sprintf'd it into a value
if(get_uart2_flag()){
set_uart2_flag(0); // set the flag to 0 to be ready for the next message
com = get_uart2_value();
sprintf(message2,"%d\r\n",com);
NU32DIP_WriteUART1(message2);
}
The PIC needs to control the left and right drive motors to keep the COM in the middle of the bitmap. Each motor PWM duty cycle will take the COM as an input, and a simple proportional controller can be used to set the duty (and you can add derivative and integral if necessary). A trick here is to use a nonlinear gain for the proportional controller. For the left motor, if the COM is too far to the left of the bitmap, the duty should be lower, which will turn the robot to the left. If the COM is too far to the right, the duty should be higher, to turn the robot to the right. If the COM is in the middle, then the duty should be high, full steam ahead! The slope of the line, the intercept, min and max values are all part of the P gain.
If you want, you can have the Pico send multiple COM values, from different rows of the bitmap. These can be used to scale the P gain, based on if the COM out further from the front of the robot is off to one side, essentially slowing the robot as it approaches a curve.
For this assignment, write code for the Pico to find the line, and print the COM to the computer. Upload your code to a folder called HW15 to your repo, and submit a video on canvas showing how the COM changes as you move the camera over a line.
The final part of the code, printing to the PIC and having the PIC follow the line, is for HW16.