Initial

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])

main.py

@@ -12,7 +12,9 @@
 from time import sleep
 from picamera import PiCamera
 from rainbow import Rainbow
-
+from grove_oled import OLED
+import grovepi
+import threading
 
 
 L1_BUTTON = 6
@@ -29,6 +31,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 +57,8 @@ 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")
+        self.length = 30
         super().__init__()
 
     def run(self):
@@ -90,22 +95,59 @@ 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)
 
     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.straight_line_start:
-            # start
-            self.bot.move(1.0, 1.0)
-        else:
-            # stop
+        if self.oled.data != "Straight":
+            self.oled.putString("Straight")
+            LOGGER.debug("Straight mode")
+        while self.straight_line_start:
+            # 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()
+
+
+    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__':

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):