From 4e50687d63bf413785e1375e5f382be1e6a8a896 Mon Sep 17 00:00:00 2001 From: Godley Date: Sat, 14 Apr 2018 17:04:22 +0100 Subject: [PATCH 1/3] feat: shift grove sample into class and use it where appropriate --- grove_oled.py | 224 +++++++++++++++++++++++++------------------------- main.py | 24 +++++- 2 files changed, 135 insertions(+), 113 deletions(-) diff --git a/grove_oled.py b/grove_oled.py index 95cb6a6..71a4cdd 100644 --- a/grove_oled.py +++ b/grove_oled.py @@ -50,17 +50,6 @@ import math import struct -if sys.platform == 'uwp': - import winrt_smbus as smbus - bus = smbus.SMBus(1) -else: - import smbus - import RPi.GPIO as GPIO - rev = GPIO.RPI_REVISION - if rev == 2 or rev == 3: - bus = smbus.SMBus(1) - else: - bus = smbus.SMBus(0) grayH= 0xF0 grayL= 0x0F @@ -169,116 +158,127 @@ [0x00,0x02,0x01,0x01,0x02,0x01,0x00,0x00], [0x00,0x02,0x05,0x05,0x02,0x00,0x00,0x00]] -def sendCommand(byte): - try: - block=[] - block.append(byte) - return bus.write_i2c_block_data(address,Command_Mode,block) - except IOError: - print("IOError") - return -1 -def sendData(byte): - try: - block=[] - block.append(byte) - return bus.write_i2c_block_data(address,Data_mode,block) - except IOError: - print("IOError") - return -1 +class OLED(object): + def __init__(self): + if sys.platform == 'uwp': + import winrt_smbus as smbus + self.bus = smbus.SMBus(1) + else: + import smbus + import RPi.GPIO as GPIO + rev = GPIO.RPI_REVISION + if rev == 2 or rev == 3: + self.bus = smbus.SMBus(1) + else: + self.bus = smbus.SMBus(0) + blk = [0xFD] # Unlock OLED driver IC MCU interface from entering command. i.e: Accept commands + blk.append(0x12) + blk.append(0xAE) # Set display off + blk.append(0xA8) # set multiplex ratio + blk.append(0x5F) # 96 + blk.append(0xA1) # set display start line + blk.append(0x00) + blk.append(0xA2) # set display offset + blk.append(0x60) + blk.append(0xA0) # set remap + blk.append(0x46) + blk.append(0xAB) # set vdd internal + blk.append(0x01) # + blk.append(0x81) # set contrasr + blk.append(0x53) # 100 nit + blk.append(0xB1) # Set Phase Length + blk.append(0X51) # + blk.append(0xB3) # Set Display Clock Divide Ratio/Oscillator Frequency + blk.append(0x01) + blk.append(0xB9) # + blk.append(0xBC) # set pre_charge voltage/VCOMH + blk.append(0x08) # (0x08) + blk.append(0xBE) # set VCOMH + blk.append(0X07) # (0x07) + blk.append(0xB6) # Set second pre-charge period + blk.append(0x01) # + blk.append(0xD5) # enable second precharge and enternal vsl + blk.append(0X62) # (0x62) + blk.append(0xA4) # Set Normal Display Mode + blk.append(0x2E) # Deactivate Scroll + blk.append(0xAF) # Switch on display + self.multi_comm(blk) + time.sleep(.1) + + # Row Address + blk = [0x75] # Set Row Address + blk.append(0x00) # Start 0 + blk.append(0x5f) # End 95 + # Column Address + blk.append(0x15) # Set Column Address + blk.append(0x08) # Start from 8th Column of driver IC. This is 0th Column for OLED + blk.append(0x37) # End at (8 + 47)th column. Each Column has 2 pixels(segments) + self.multi_comm(blk) -def multi_comm(commands): - for c in commands: - sendCommand(c) + def sendCommand(self, byte): + try: + block=[byte] + return self.bus.write_i2c_block_data(address,Command_Mode,block) + except IOError: + print("IOError") + return -1 -# Init function of the OLED -def oled_init(): - blk=[0xFD] # Unlock OLED driver IC MCU interface from entering command. i.e: Accept commands - blk.append(0x12) - blk.append(0xAE) # Set display off - blk.append(0xA8) # set multiplex ratio - blk.append(0x5F) # 96 - blk.append(0xA1) # set display start line - blk.append(0x00) - blk.append(0xA2) # set display offset - blk.append(0x60) - blk.append(0xA0) # set remap - blk.append(0x46) - blk.append(0xAB) # set vdd internal - blk.append(0x01) # - blk.append(0x81) # set contrasr - blk.append(0x53) # 100 nit - blk.append(0xB1) # Set Phase Length - blk.append(0X51) # - blk.append(0xB3) # Set Display Clock Divide Ratio/Oscillator Frequency - blk.append(0x01) - blk.append(0xB9) # - blk.append(0xBC) # set pre_charge voltage/VCOMH - blk.append(0x08) # (0x08) - blk.append(0xBE) # set VCOMH - blk.append(0X07) # (0x07) - blk.append(0xB6) # Set second pre-charge period - blk.append(0x01) # - blk.append(0xD5) # enable second precharge and enternal vsl - blk.append(0X62) # (0x62) - blk.append(0xA4) # Set Normal Display Mode - blk.append(0x2E) # Deactivate Scroll - blk.append(0xAF) # Switch on display - multi_comm(blk) - time.sleep(.1) + def sendData(self, byte): + try: + block=[byte] + return self.bus.write_i2c_block_data(address,Data_mode,block) + except IOError: + print("IOError") + return -1 - # Row Address - blk=[0x75] # Set Row Address - blk.append(0x00) # Start 0 - blk.append(0x5f) # End 95 - # Column Address - blk.append(0x15) # Set Column Address - blk.append(0x08) # Start from 8th Column of driver IC. This is 0th Column for OLED - blk.append(0x37) # End at (8 + 47)th column. Each Column has 2 pixels(segments) - multi_comm(blk) + def multi_comm(self, commands): + for c in commands: + self.sendCommand(c) -def oled_clearDisplay(): - for j in range (0,48): - for i in range (0,96): - sendData(0x00) + def clear(self): + for j in range (0,48): + for i in range (0,96): + self.sendData(0x00) -def oled_setNormalDisplay(): - sendCommand(Normal_Display_Cmd) + def setNormalDisplay(self): + self.sendCommand(Normal_Display_Cmd) -def oled_setVerticalMode(): - sendCommand(0xA0) # remap to - sendCommand(0x46) # Vertical mode + def setVerticalMode(self): + self.sendCommand(0xA0) # remap to + self.sendCommand(0x46) # Vertical mode -def oled_setTextXY(Row,Column): - sendCommand(0x15) # Set Column Address - sendCommand(0x08+(Column*4)) # Start Column: Start from 8 - sendCommand(0x37) # End Column - # Row Address - sendCommand(0x75) # Set Row Address - sendCommand(0x00+(Row*8)) # Start Row - sendCommand(0x07+(Row*8)) # End Row + def setTextXY(self, Row,Column): + self.sendCommand(0x15) # Set Column Address + self.sendCommand(0x08+(Column*4)) # Start Column: Start from 8 + self.sendCommand(0x37) # End Column + # Row Address + self.sendCommand(0x75) # Set Row Address + self.sendCommand(0x00+(Row*8)) # Start Row + self.sendCommand(0x07+(Row*8)) # End Row -def oled_putChar(C): - C_add=ord(C) - if C_add<32 or C_add>127: # Ignore non-printable ASCII characters - C=' ' + def putChar(self, C): C_add=ord(C) + if C_add<32 or C_add>127: # Ignore non-printable ASCII characters + C=' ' + C_add=ord(C) - for i in range(0,8,2): - for j in range(0,8): - c=0x00 - bit1=((BasicFont[C_add-32][i])>>j)&0x01 - bit2=((BasicFont[C_add-32][i+1])>>j)&0x01 - if bit1: - c=c|grayH - else: - c=c|0x00 - if bit2: - c=c|grayL - else: - c=c|0x00 - sendData(c) + for i in range(0,8,2): + for j in range(0,8): + c=0x00 + bit1=((BasicFont[C_add-32][i])>>j)&0x01 + bit2=((BasicFont[C_add-32][i+1])>>j)&0x01 + if bit1: + c=c|grayH + else: + c=c|0x00 + if bit2: + c=c|grayL + else: + c=c|0x00 + self.sendData(c) -def oled_putString(String): - for i in range(len(String)): - oled_putChar(String[i]) + def putString(self, String): + self.data = String + for i in range(len(String)): + self.putChar(String[i]) diff --git a/main.py b/main.py index 4412260..10e9364 100644 --- a/main.py +++ b/main.py @@ -12,7 +12,8 @@ from time import sleep from picamera import PiCamera from rainbow import Rainbow - +from grove_oled import OLED +import grovepi L1_BUTTON = 6 @@ -29,6 +30,7 @@ class Controller(Rainbow): mode = R1_BUTTON def __init__(self, amybot=True, cambot=False): + self.oled = OLED() self.joystick = Joystick() # if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately if amybot: @@ -54,6 +56,7 @@ def __init__(self, amybot=True, cambot=False): max_power = voltage_out / float(voltage_in) self.straight_line_start = False self.modes = {L1_BUTTON: self.rainbow, R1_BUTTON: self.remote, L2_BUTTON: self.maze, R2_BUTTON: self.straight} + self.oled.putString("Remote") super().__init__() def run(self): @@ -91,14 +94,20 @@ def run(self): def remote(self): LOGGER.debug("Remote mode") + if self.oled.data != "Remote": + self.oled.putString("Remote") left_drive, right_drive = self.joystick.get_reading() self.bot.move(left_drive, right_drive) def maze(self): + if self.oled.data != "Maze": + self.oled.putString("Maze") LOGGER.debug("Maze mode") def straight(self): LOGGER.debug("Straight mode") + if self.oled.data != "Straight": + self.oled.putString("Straight") if self.straight_line_start: # start self.bot.move(1.0, 1.0) @@ -106,6 +115,19 @@ def straight(self): # stop self.bot.stop() + def read_distance(self): + + # Connect the Grove Ultrasonic Ranger to digital port D7 + # SIG,NC,VCC,GND + ultrasonic_ranger = 7 + try: + # Read distance value from Ultrasonic + return grovepi.ultrasonicRead(ultrasonic_ranger) + + except TypeError: + print("Error") + except IOError: + print("Error") if __name__ == '__main__': From c08ca2e7910ad0372b568c2b7889743e2b31f36d Mon Sep 17 00:00:00 2001 From: Godley Date: Sat, 14 Apr 2018 17:18:33 +0100 Subject: [PATCH 2/3] feat: make the straight line bit a bit smarter --- main.py | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/main.py b/main.py index 10e9364..04caa36 100644 --- a/main.py +++ b/main.py @@ -57,6 +57,7 @@ def __init__(self, amybot=True, cambot=False): self.straight_line_start = False self.modes = {L1_BUTTON: self.rainbow, R1_BUTTON: self.remote, L2_BUTTON: self.maze, R2_BUTTON: self.straight} self.oled.putString("Remote") + self.length = 30 super().__init__() def run(self): @@ -103,20 +104,31 @@ def maze(self): if self.oled.data != "Maze": self.oled.putString("Maze") LOGGER.debug("Maze mode") + while True: + data = self.read_distance() + if data < self.length: + self.bot.move(1.0, 0.0) + else: + self.bot.move(1.0, 1.0) def straight(self): LOGGER.debug("Straight mode") - if self.oled.data != "Straight": - self.oled.putString("Straight") - if self.straight_line_start: - # start + while self.straight_line_start: self.bot.move(1.0, 1.0) - else: - # stop - self.bot.stop() + data = self.read_distance() + mv_left = 1.0 + mv_right = 0.0 + while data < self.length: + self.bot.stop() + new_data = self.read_distance() + if new_data < data: + mv_left = 0.0 + mv_right = 1.0 + self.bot.move(mv_left, mv_right) + data = self.read_distance() - def read_distance(self): + def read_distance(self): # Connect the Grove Ultrasonic Ranger to digital port D7 # SIG,NC,VCC,GND ultrasonic_ranger = 7 From 5d048a82796d8748d1059187692f40acaf8f46c6 Mon Sep 17 00:00:00 2001 From: Godley Date: Sun, 15 Apr 2018 13:08:24 +0100 Subject: [PATCH 3/3] latest code --- main.py | 34 +++++++++++++++++++++------------- rainbow.py | 1 - 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/main.py b/main.py index 04caa36..3b7ddb3 100644 --- a/main.py +++ b/main.py @@ -14,6 +14,7 @@ from rainbow import Rainbow from grove_oled import OLED import grovepi +import threading L1_BUTTON = 6 @@ -94,8 +95,9 @@ def run(self): LOGGER.debug('Motors off') def remote(self): - LOGGER.debug("Remote mode") + if self.oled.data != "Remote": + LOGGER.debug("Remote mode") self.oled.putString("Remote") left_drive, right_drive = self.joystick.get_reading() self.bot.move(left_drive, right_drive) @@ -112,20 +114,26 @@ def maze(self): self.bot.move(1.0, 1.0) def straight(self): - LOGGER.debug("Straight mode") + if self.oled.data != "Straight": + self.oled.putString("Straight") + LOGGER.debug("Straight mode") while self.straight_line_start: - self.bot.move(1.0, 1.0) + # may want to do threading here if this is non-reactive + self.straight_action() + + def straight_action(self): + self.bot.move(1.0, 1.0) + data = self.read_distance() + mv_left = 1.0 + mv_right = 0.0 + while data < self.length: + self.bot.stop() + new_data = self.read_distance() + if new_data < data: + mv_left = 0.0 + mv_right = 1.0 + self.bot.move(mv_left, mv_right) data = self.read_distance() - mv_left = 1.0 - mv_right = 0.0 - while data < self.length: - self.bot.stop() - new_data = self.read_distance() - if new_data < data: - mv_left = 0.0 - mv_right = 1.0 - self.bot.move(mv_left, mv_right) - data = self.read_distance() def read_distance(self): diff --git a/rainbow.py b/rainbow.py index 453d74b..3b67c1f 100644 --- a/rainbow.py +++ b/rainbow.py @@ -91,7 +91,6 @@ def process(self, image, colour, debug=False): # Yellow is... to be found! imrange = cv2.inRange(image, np.array(ranges[colour]["low"]), np.array(ranges[colour]["high"])) - cv2.erode(imrange, ) # I used the following code to find the approximate 'hue' of the ball in # front of the camera # for crange in range(0,170,10):