Yong Voon Siew
Published © GPL3+

Autonomous SanitizerOnDemand Robot with OpenCV Navigation

Smart Robot help to protect building residents in high rise apartment's access and common area with SanitizerOnDemand and DeliverySecured.

AdvancedFull instructions providedOver 3 days3,585

Things used in this project

Hardware components

Arduino UNO
Arduino UNO
×3
NodeMCU ESP8266 Breakout Board
NodeMCU ESP8266 Breakout Board
×1
Raspberry Pi 3 Model B
Raspberry Pi 3 Model B
×1
Cytron Technologies Motor driver MDD10A
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
×3
Geared DC Motor, 12 V
Geared DC Motor, 12 V
×4
Webcam, Logitech® HD Pro
Webcam, Logitech® HD Pro
×2
Lipo Battery 3S
×2
T-Slot Aluminium
×1
Jumper wires (generic)
Jumper wires (generic)
×3
FPV camera
×1
FPV Receiver
×1

Software apps and online services

Raspbian
Raspberry Pi Raspbian
Visual Studio 2017
Microsoft Visual Studio 2017
Arduino IDE
Arduino IDE
OpenCV
OpenCV
SketchUp

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Hot glue gun (generic)
Hot glue gun (generic)
Plier, Long Nose
Plier, Long Nose
Multitool, Screwdriver
Multitool, Screwdriver

Story

Read more

Custom parts and enclosures

Linear Servo Feedback Loop.xlsx

View in Excel for the Servo performance

Robot 3D Sketchup of Frame and Components

Linear Servo Schematic

Navigation QR Codes

Navigation QR Codes in printed format

Schematics

DeliveryBox Schematic

Ultrasonic Schematic

Motordriver & radio receiver

Linear Servo Schematic

Linear Servo and Motor driver and sensor Schematic

Code

robotarm_i2c_mist_pump_10.ino

Arduino
Arduino Uno code for linear servo, angular arm servo and Mist driver
#include <Servo.h>
#include <Wire.h>
Servo servo1;
int pwmservo = 6;
int endswitch = 13;

int motorpwm = 5;
int IN3 = 4;
int IN4 = 7;

int pumppwm = 11;
int IN1 = 10 ;
int IN2 = 8;

int encoder1 = 2;
int encoder2 = 3;
int mister = 9;

int encodercount =0;
int bigcount = 0;
int positionstate=  0;
#define up  1
#define down  0
#define halt  3
#define encodermax 3587
#define encodermin 0

int i2cdata[32];              //i2cdata[0] = index 0 is the register offset. useful for selection and commands
int i2cnumbytes=32;           //number of bytes in i2c
int i2c_robotarm_enable = 0;
int servoangle =0;

void setup() {
  
  Wire.begin(0x9); // Start I2C on Address 0x09
  Wire.onReceive(receiveEvent); // Receive message from RPI - write to this Slave Arduino
  Wire.onRequest(requestEvent); // Sending information back to the RPI
  
  Serial.begin(115200);
  Serial.println("RobotArmLiftPresser");

  pinMode(mister,INPUT);     //set Input to Mister gpio as high impedance input
  delay(1000);
  pinMode(mister,OUTPUT);     //set Output to Mister gpio as low impedance output
  //digitalWrite(mister,HIGH);
  
//  setPwmFrequency(3, 1);    //change PWM1 = 31.25kHz
  setPwmFrequency(5, 1);    //change PWM2 = 62.5kHz
//  setPwmFrequency(6, 1);    //change PWM3 = 62.5kHz
//  setPwmFrequency(9, 1);    //change PWM4 = 62.5kHz

  /*   for(int i=0;i<=10;i++){    // 1 series of pulse to turn on Mister. 
    digitalWrite(mister,HIGH);
    delay(1);
    digitalWrite(mister,LOW);
    delay(1);
    }
   digitalWrite(mister,HIGH);
  */
  
  servo1.attach(pwmservo);
  servo1.write(95);
  
  pinMode(endswitch,INPUT_PULLUP);
  pinMode(motorpwm,OUTPUT);   //linear dc motor
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  analogWrite(motorpwm,10);

  pinMode(pumppwm,OUTPUT);      //water pump
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  //analogWrite(pumppwm,11);      // min PWM to turn on Pump
  //  while(1){
  //waterpump(1,15);
  //Serial.println("On Pump");
  //delay(300000);
  //waterpump(0,0);
  //Serial.println("Off Pump");
  //delay(300000);   
  //}
  
  pinMode(encoder1,INPUT);
  pinMode(encoder2,INPUT);
  attachInterrupt(digitalPinToInterrupt(encoder1),count,CHANGE);
  Serial.println();
  Serial.println("Homing");

  while(digitalRead(endswitch)==1)
    {
    motor(down,140);
    delay(150);
    motor(halt,0);
    delay(50);
    }
  
  motor(halt,0);
  delay(20000);

  encodercount=0;             //reset encoder. encoder takes 3587 counts end to end
  Serial.println("Reached");   
  Serial.println("Motor Centered");   
  //motorposition(encodermax/2,190);
  delay(10000);
  positionstate= 1800;
  servo1.write(5);
  mist_on();
  Serial.println("Mist Primed");
}

void loop() {


int endstop;
int incomingByte;
   //i2cdata[0] = 4 // write to position and servo with i2cdata[1] + i2cdata[2] and servo angle = i2cdata[3]
   //i2cdata[0] = 5 // write only to servo with i2cdata[1] only.
   //i2cdata[0] = 6 // write for home linear arm and reset encoder and positionstate
   //i2cdata[0] = 7 // write to return back encodercount value to RPI for reading
   //i2cdata[0] = 8 // write to turn on /led/off mist.
   //i2cdata[0] = 9 // write to turn on /off waterpump.
   
    
    if (i2cdata[0]==4){         // i2c command to write to linear servo position
    positionstate = i2cdata[1] + (i2cdata[2] << 8);      //pass master's i2c data for new positioning from double byte into 16bit integer
    if(i2cdata[3]<=180){
      servo1.write(i2cdata[3]);
     i2cdata[0]=0;
     }
    }
    
    if(i2cdata[0]==5){          // i2c command to write to servo position
      servo1.write(i2cdata[1]);
      i2cdata[0]=0;
     }

    if(i2cdata[0]==6){            // i2c command to reset home position
        Serial.println("Homing...");
        servo1.write(5);
        while(digitalRead(endswitch)==1)
            {
            motor(down,140);
            delay(150);
            motor(halt,0);
            delay(50);
            }
       motor(halt,0);
       encodercount=0;              // reset encoder
       positionstate=1700;          // reset positioinstate slightly higher
       delay(2000);
       Serial.println("Home");
       i2cdata[0]=0;                //clear data
    }

    if(i2cdata[0]==8){              // i2c command to turn on Mist_on
      mist_on();
      i2cdata[0]=0;                //clear data
    }

    if(i2cdata[0]==9){                  // i2c command to turn on waterpump
      waterpump(i2cdata[1],i2cdata[2]);
      i2cdata[0]=0;                    //clear data
    }

    if(encodercount>=positionstate+50 || encodercount <=positionstate-50){   //Hysteris to eliminate whining noise during lock position.
      motorposition(positionstate,240);                                       // move to position.
      }
    else if (encodercount>=positionstate+10 || encodercount <=positionstate-10){   //Hysteris to eliminate whining noise during lock position.
      motorposition(positionstate,200);                                       // move to position.
      }

  
    if(Serial.available()>0){       //for debugging purposes via serial monitor.
        incomingByte=Serial.read();
        if(incomingByte=='8')
           {
            positionstate=positionstate+50;
            if(positionstate>=encodermax-50){
            positionstate=encodermax-50;
           }
      }
    else if(incomingByte=='2'){   
        positionstate=positionstate-50;
        if(positionstate<=encodermin+50)
           {
           positionstate=encodermin+50;
             }
        }
    
   else if(incomingByte=='m'){    // on /led/off/ mist
        mist_on();
        Serial.println("Mister");
      }
   
   else if(incomingByte=='w'){    //on min water pump 
        waterpump(1,15);
      }
   else if(incomingByte=='s'){    //off water pump
        waterpump(0,0);
      }
   else if(incomingByte=='e'){    //on max water pump
        waterpump(1,200);
      }
    }
  
}

///////////////////////// Subroutines ////////////////////////


void count(){                                   // ISR routine for interrupt pin 2 encoder signal


  if(digitalRead(encoder1)!=digitalRead(encoder2)) //quadrature encoder for detecting motor direction.
    {
    encodercount++;                             //increment encoder value if moving up
    }
  else
    {
    encodercount--;                             //decrement encoder value if moving down
    }
  
}


void motor(int direction,int velocity){
  if(direction == up)
  {
     analogWrite(motorpwm,velocity);
     digitalWrite(IN3,HIGH);
     digitalWrite(IN4,LOW);
  }
  else if( direction == down)
  {
     analogWrite(motorpwm,velocity);
     digitalWrite(IN3,LOW);
     digitalWrite(IN4,HIGH);
  }
  else if( direction == halt)
  {
     analogWrite(motorpwm,0);
     digitalWrite(IN3,LOW);
     digitalWrite(IN4,LOW);
  }

}

void motorposition(int encoderposition,int velocity){
   
   if (encodercount<encoderposition+5)
     {
      motor(up,velocity);
      while (encodercount<=encoderposition){
      Serial.println(encodercount);
      }
     }
   else if(encodercount>encoderposition-5)
    {
     motor(down,140);
     while (encodercount>encoderposition){
     Serial.println(encodercount);
     
      }
     }
   motor(halt,0);
   
   //Serial.println("Halt");
   
}

void setPwmFrequency(int pin, int divisor) {
  byte mode;
  if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 64: mode = 0x03; break;
      case 256: mode = 0x04; break;
      case 1024: mode = 0x05; break;
      default: return;
    }
    if(pin == 5 || pin == 6) {
      TCCR0B = TCCR0B & 0b11111000 | mode;
    } else {
      TCCR1B = TCCR1B & 0b11111000 | mode;
    }
  } else if(pin == 3 || pin == 11) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 32: mode = 0x03; break;
      case 64: mode = 0x04; break;
      case 128: mode = 0x05; break;
      case 256: mode = 0x06; break;
      case 1024: mode = 0x07; break;
      default: return;
    }
    TCCR2B = TCCR2B & 0b11111000 | mode;
  }
}

void mist_on(){
  pinMode(mister,INPUT);     //set Input to Mister gpio as high impedance input
  delay(3000);
  pinMode(mister,OUTPUT);     //set Output to Mister gpio as low impedance output
  digitalWrite(mister,LOW);
  delay(3000);
  
   for(int i=0;i<=100;i++){
      digitalWrite(mister,HIGH);  //generate simulated finger touch pulses.
      delay(1);
      digitalWrite(mister,LOW);
      delay(1);
    }
    digitalWrite(mister,HIGH);    //set high to remain last touch pulses.
 
  
}

void waterpump(int on_off, int speed){

 if(on_off==1){
    analogWrite(pumppwm,speed);      // PWM to turn on Pump
    digitalWrite(IN1,HIGH);
    digitalWrite(IN2,LOW);
    Serial.println("WaterPump On");
   }
 else if(on_off==0){
    analogWrite(pumppwm,0);          // PWM to turn off Pump
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    Serial.println("WaterPump Off");
   }
    
}
void requestEvent() {

  if(i2cdata[0]==7){          // #7 query for encodercount
   Wire.write(encodercount & 0xFF);  // respond with message encodercount
   Wire.write(encodercount >> 8);
   Wire.write(servo1.read());
   //Serial.println("Sending...");
   i2cdata[0]=0;
  }
 
}


// function that executes whenever data is received from master
//  slave device receives a transmission from a master.
int receiveEvent(int howMany) {
  int i;
   i2cnumbytes=howMany;
   for(i=0;i<howMany;i++){
      i2cdata[i]= Wire.read();        // read the offset register first.
      //int c = Wire.read();    
      if(howMany>1) {
       //   Serial.print(i2cdata[i]);          
       //   Serial.print(",");
        }
    }
    if(howMany>1) {
     //    Serial.print("WriteReg=[");
     //    Serial.print(i2cdata[0]);
     //    Serial.print("] :Num:");
     //    Serial.println(howMany);         
           if(i2cdata[0]== 4 ){
            i2c_robotarm_enable = 1;
            }
      //     else if(i2cdata[0]==5)
     //      {
     //       servo1.write(i2cdata[1]);
     //       Serial.println("servo");
     //      }
    }
 
}

navigationlocation5.py

Python
Use QR Code as near-navigation and Optical Character Recognition for Lift button identification and control. Return x, y coordinates including area, height and width of the detected QRcode and Lift Number position.
import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
from imutils.video import FPS
import pytesseract
from pyzbar import pyzbar

#pytesseract = pytesseract.tesseract_cmd = r'C:\\Program Files (x86)\\Tesseract-OCR\\tesseract.exe'

graphic_enable=0
graphic_off = 0
graphic_on = 1
screen_width = 720 #720 #1280 #720 #640 #500
screen_hight =  405 #405 #720 #405 #360 #281
textsetting ="-c tessedit","_char_whitelist=abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"," --psm 10"," -l osd"," " # detect single character
def camera_setup():
    print("Start Camera QR Code & Text Recognition!")
    global cam
    cam = cv2.VideoCapture(0)  

def camerasearch(graphic_enable = graphic_off):
        global lowerBounddd
        #global upperBound
        #global img,ret,gray
        #global contours,edged,auto,blurred
        #ret, img = cam.read()
        #for i in range(1):
        #    ret, img = cam.read()
        #    img = cv2.resize(img, (screen_width, screen_hight))
        #    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 
        #    gray = cv2.GaussianBlur(gray, (3, 3), 0)     
         #   #gray = cv2.bitwise_not(gray)
         #  #ret,gray = cv2.threshold(gray,127,255,cv2.THRESH_BINARY)
            #gray = cv2.adaptiveThreshold(gray,255,cv2.ADAPTIVE_THRESH_MEAN_C,cv2.THRESH_BINARY,11,2)
            #gray = cv2.adaptiveThreshold(gray,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,cv2.THRESH_BINARY,17,4)
            
            #edged = cv2.Canny(gray, 130, 200) 
            #contours, hierarchy = cv2.findContours(edged, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            #cv2.drawContours(img, contours, -1, (0, 0, 255), 2) 
            
def camera_close():
    cam.release()
    cv2.destroyAllWindows()     

def readqr(graphic_enable = 0):
        global lowerBound
        global upperBound
        global img,ret,gray
        global contours,edged,auto,blurred
        ret, img = cam.read()
        for i in range(1):
            ret, img = cam.read()
            img = cv2.resize(img, (screen_width, screen_hight))
            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 
            gray = cv2.GaussianBlur(gray, (3, 3), 0)     
            #gray = cv2.bitwise_not(gray)
            #ret,gray = cv2.threshold(gray,127,255,cv2.THRESH_BINARY)
            #gray = cv2.adaptiveThreshold(gray,255,cv2.ADAPTIVE_THRESH_MEAN_C,cv2.THRESH_BINARY,11,2)
            #gray = cv2.adaptiveThreshold(gray,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,cv2.THRESH_BINARY,17,4)
            
            #edged = cv2.Canny(gray, 130, 200) 
            #contours, hierarchy = cv2.findContours(edged, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            #cv2.drawContours(img, contours, -1, (0, 0, 255), 2) 
        global qx,qy,qw,qh,barcodeData
        barcodes = pyzbar.decode(gray)
        if barcodes is None:
            x=0
            y=0
            h=0
            w=0
            area=0
            barcodeData='-'
        for barcode in barcodes:
            (x, y, w, h) = barcode.rect
            qx=x+round(w/2)
            qy=y+round(h/2)
            cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 255), 2)
            cv2.rectangle(img, (qx-5, qy-5), (qx+5, qy+5), (0, 255, 0), 2)
            barcodeData = barcode.data.decode("utf-8")
            barcodeType = barcode.type
            text = "{} ({})".format(barcodeData, barcodeType)
            cv2.putText(img, text, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX,0.5, (0, 0, 255), 2)
            #print("Found {} barcode: {}".format(barcodeType, barcodeData))
            print(barcodeData)
            #cv2.imshow("QRcode", img)
            showInMovedWindow('QRCode',img,10,50,graphic_enable)
            print(qx,qy)
            return(qx,qy,barcodeData,h,w)


def charsearch(what=49, graphic_enable=graphic_on):
    global ch,cw,cy,cx,b,img,count,barray
    #text = pytesseract.image_to_string(gray,config='--psm 13')
    cx=0
    cy=0
    ch=0
    cw=0
    barray=np.array([])
    count=0
    boxes= pytesseract.image_to_boxes(gray,config='--psm 13')
    h, w, c  = img.shape
    barray=boxes
    for b in boxes.splitlines():
        b = b.split(' ')
        if (b[0]==chr(what)):
            img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
            cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
            cx=int(b[1])
            cy=h-int(b[2])
            ch=int(b[4])-int(b[2])
            cw=int(b[3])-int(b[1])
            count=count+1          
            print("cx,cy,ch,cw:",cx,cy,ch,cw)
    
    showInMovedWindow('Char',img,710,50,graphic_enable)      
    print(count)
        

def charsearchdata(what=49, graphic_enable=graphic_on):
    data= pytesseract.image_to_data(gray,config='--psm 13')
    print(data)   

def showInMovedWindow(winname, img, x, y,graphic_enable= graphic_off):
    if(graphic_enable==graphic_on):
        cv2.namedWindow(winname)        # Create a named window
        cv2.moveWindow(winname, x, y)   # Move it to (x,y)
        cv2.imshow(winname,img)                       



if (__name__=="__main__"):
    camera_setup()
    while 1:
        #camerasearch(graphic_on)
        readqr(graphic_on)
        #showInMovedWindow('QRCode',img,10,50,graphic_on)
        k=cv2.waitKey(1) 
        if k == ord('x'):
            cv2.destroyWindow("QRCode")
            break
        elif k != -1 and k!=ord('s'):
            print(chr(k))
            charsearch(k)
            #xyz=np.where(barray==ord('4'))
            #print(xyz)
            #print(barray) 
            #showInMovedWindow('Char',img,710,50,graphic_on)
        elif k == ord('s'):
            print("Your character 's'")
            charsearchdata(1)
            #charsearch(ord('4'))
            #showInMovedWindow('Char',img,710,50,graphic_on)

    camera_close()
    


if(__name__=="__main__"):
    camera_setup()
    while(1):
        camerasearch(graphic_on)
        cv2.imshow("Contour",img)
        cv2.imshow("Gray",gray)
        k=cv2.waitKey(1) 
        if k == ord('x'):
            break
        elif k== ord('2'):
            #text = pytesseract.image_to_string(gray,config='--psm 13 ')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 13 ')
            h, w, c  = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='2'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)
        elif k== ord('1'):
            #text = pytesseract.image_to_string(gray,config='--psm 13')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 13')
            h, w, c = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='1'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)
        elif k== ord('3'):
            #text = pytesseract.image_to_string(gray,config='--psm 13')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 13')
            h, w, c  = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='3'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)
        elif k== ord('l'):
            #text = pytesseract.image_to_string(gray,config='--psm 13')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 13')
            h, w, c  = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='L'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)                    
        elif k== ord('6'):
            #text = pytesseract.image_to_string(gray,config='--psm 12')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 12')
            h, w, c  = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='6'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)                    
        elif k== ord('g'):
            #text = pytesseract.image_to_string(gray,config='--psm 13')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 13')
            h, w, c  = img.shape
            for b in boxes.splitlines():
                b = b.split(' ')
                if (b[0]=='G'):
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 2)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)                    
        elif k== ord('a'):
            #text = pytesseract.image_to_string(gray,config='--psm 11 --oem 3 -c tessedit_char_whitelist=0123456789')
            #boxes= pytesseract.image_to_boxes(gray,config='--psm 11 --oem 3 -c tessedit_char_whitelist=0123456789')
            #text = pytesseract.image_to_string(gray,config='--psm 12 --oem 2')
            boxes= pytesseract.image_to_boxes(gray,config='--psm 12 --oem 2')
            h, w, c = img.shape
            for b in boxes.splitlines():
                if (b[0]!='~' ):  #and b[0].isdigit()
                    b = b.split(' ')
                    img = cv2.rectangle(img, (int(b[1]), h - int(b[2])), (int(b[3]), h - int(b[4])), (255, 0, 0), 1)
                    cv2.putText(img, str(b[0]), (int(b[1]),h - int(b[2])+10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
                    cv2.imshow("Boxed",img)
                    print(b)
        elif k==ord('q'):
            barcodes = pyzbar.decode(gray)
            #print(barcodes)
            for barcode in barcodes:
                (x, y, w, h) = barcode.rect
                qx=x+round(w/2)
                qy=y+round(h/2)
                cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 255), 2)
                cv2.rectangle(img, (qx-5, qy-5), (qx+5, qy+5), (0, 255, 0), 2)
                barcodeData = barcode.data.decode("utf-8")
                barcodeType = barcode.type
                text = "{} ({})".format(barcodeData, barcodeType)
                cv2.putText(img, text, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX,0.5, (0, 0, 255), 2)
                print("Found {} barcode: {}".format(barcodeType, barcodeData))
                #
                # print(barcodeData)
                cv2.imshow("QRcode", img)

    camera_close()

ultrasonic_i2C_rgb_pump.ver5.ino

Arduino
Arduino Uno code for ultrasonic proximity detection, LED driver, water pump driver, and i2C bus powered
#include <Wire.h>
#include <FastLED.h>

int i2cdata[32];              //i2cdata[0] = index 0 is the register offset. useful for selection and commands
int i2cnumbytes=32;           //number of bytes in i2c

int detect1,detect2,detect3,detect4;
int timeout = 20000;  //timeout 20ms
int cm_previous = 0; 

int trigPin1 = 13;    // Trigger
int echoPin1 = 12;    // Echo
long duration1, cm1;

int trigPin2 = 11;    // Trigger
int echoPin2 = 10;    // Echo
long duration2, cm2;

int trigPin3 = A2;    // Trigger
int echoPin3 = A3;    // Echo
long duration3, cm3;

int trigPin4 = 9;    // Trigger
int echoPin4 = 8;    // Echo
long duration4, cm4;

int ledpwm = 3;
int motorpwm = 5;

#define NUM_LEDS 8
#define DATA_PIN 7
CRGB leds[NUM_LEDS];
CRGBPalette16 currentPalette;
TBlendType    currentBlending;
int brightness = 50;
int detectiondistance = 25;

int i=0;
int serial_enable = 0;
int led_enable = 0;
int ledrgb_enable = 0;
int ledrgbdetect_enable =1;
int motor_enable =0;

void setup() {
  Wire.begin(8);                // join i2c bus with address #8
  Wire.onRequest(requestEvent); // register event
  Wire.onReceive(receiveEvent); // register event

  //Serial Port begin
  Serial.begin(115200);
  Serial.println("Ultrasonic,LED & RGB I2C Slave Ready: Add 0x08");

  //Define inputs and outputs
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  //Define inputs and outputs
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  //Define inputs and outputs
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  //Define inputs and outputs
  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);
  
  
  analogWrite(ledpwm,1);     //start with dimm light .
  //analogWrite(motorpwm,150);     //on motor pump 

  
  FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(brightness);
  currentPalette = RainbowColors_p;
  for(int i;i<1000;i++){
    FillLEDsFromPaletteColors(i,80);
    FastLED.show();
  }
  brightness = 15;
  ledrgb(CRGB::Blue,brightness,0,1);
  ledrgb(CRGB::Black,15,2,5);
  ledrgb(CRGB::Blue,brightness,6,7);

  analogWrite(motorpwm,0);     //off motor pump 

  
}
 
void loop() {

  
  detect1 = obstacle_sensor(trigPin1,echoPin1);
  delay(1);
  detect2 = obstacle_sensor(trigPin2,echoPin2);
  delay(1);
  detect3 = obstacle_sensor(trigPin3,echoPin3);
  delay(1);
  detect4 = obstacle_sensor(trigPin4,echoPin4);
  delay(1);

  if(ledrgbdetect_enable==1)
    {
  if(detect1<=detectiondistance){
    ledrgb(CRGB::Red,brightness,6,6);
    }
  else {
    ledrgb(CRGB::Blue,brightness,6,6);
    }
  if(detect2<=detectiondistance){
    ledrgb(CRGB::Red,brightness,1,1);
    }
  else {
    ledrgb(CRGB::Blue,brightness,1,1);
    }
  if(detect3<=detectiondistance){
    ledrgb(CRGB::Red,brightness,7,7);
    }
  else {
    ledrgb(CRGB::Blue,brightness,7,7);
    }
  if(detect4<=detectiondistance){
    ledrgb(CRGB::Red,brightness,0,0);
    }
  else {
    ledrgb(CRGB::Blue,brightness,0,0);
    }
   
}
  
if(serial_enable==1){
    Serial.print(detect1);
    Serial.print(" ");
    Serial.print(detect2);
    Serial.print(" ");
    Serial.print(detect3);
    Serial.print(" ");
    Serial.print(detect4);
    Serial.print(" ");
    Serial.println();
    //delay(50);
    serial_enable=0;
  }
if (led_enable == 1){
    analogWrite(ledpwm,i2cdata[1]);
  //  Serial.println("sss");
    led_enable = 0;
    
  }

if (motor_enable == 1){
    analogWrite(motorpwm,i2cdata[1]);
  //  Serial.println("sss");
    motor_enable = 0;
    
  }
if (ledrgb_enable == 1){
    if(i2cdata[1]==1){
      ledrgb(CRGB::White,brightness,2,5);
    }
    else if(i2cdata[1]==2){
      ledrgb(CRGB::Red,brightness,2,5);
    }
    else if(i2cdata[1]==3){
      ledrgb(CRGB::Green,brightness,2,5);
    }
    else if(i2cdata[1]==4){
      ledrgb(CRGB::Cyan,brightness,2,5);
    }
    else if(i2cdata[1]==0){
      ledrgb(CRGB::Black,brightness,2,5);
    }
    else if(i2cdata[1]==5){
      ledrgb(CRGB::Magenta,brightness,2,5);
    }

    ledrgb_enable = 0;
  }
}

int obstacle_sensor(int trigPin, int echoPin){
  int duration,cm;
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH,timeout);
  
  // Convert the time into a distance
  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  cm_previous=cm;
      if(cm>=0) {
      return(cm);
      }

}

void requestEvent() {

  if(i2cdata[0]==1){      // #1 query all ultrasonic sensor.
   // Wire.write(detect1);  // respond with message 
    Wire.write(detect1);  // respond with message 
    Wire.write(detect2);  // respond with message 
    Wire.write(detect3);  // respond with message 
    Wire.write(detect4);  // respond with message 
    ledrgbdetect_enable = 0;
  }
  else if(i2cdata[0]==2){
    Wire.write("ZZZZ  0"); // respond with message   
  }
  else {
    Wire.write(99);
  }
}


// function that executes whenever data is received from master
//  slave device receives a transmission from a master.
void receiveEvent(int howMany) {
  int i;
   // while(Wire.available()>0){
   i2cnumbytes=howMany;
   for(i=0;i<howMany;i++){
      i2cdata[i]= Wire.read();        // read the offset register first.
      //int c = Wire.read();    
      if(howMany>1) {
      //  Serial.print(i2cdata[i]);          
      //  Serial.print(",");
      }
  }
    if(howMany>1) {
   //   Serial.print(" : WriteReg=[");
   //   Serial.print(i2cdata[0]);
   //   Serial.print("] :Num:");
   //   Serial.println(howMany);         // print the integer
      if(i2cdata[0]==1){
       // serial_enable=1;
      }
      else if(i2cdata[0]==0){
      //  serial_enable=0;
      }
      else if(i2cdata[0]==2){
        led_enable = 1;
    //    Serial.print("led");
    //    Serial.println(led_enable);
      }
      else if(i2cdata[0]==3){
        ledrgb_enable = 1;
     //   Serial.print("rgbled");
     //   Serial.println(ledrgb_enable);
      }
     else if(i2cdata[0]==4){
        motor_enable = 1;
     //   Serial.print("motorpwm");
     //   Serial.println(ledrgb_enable);
      }

    }
}

void ledrgb(CRGB color,int intensity,int minbit, int maxbit){
  if (maxbit<=NUM_LEDS && minbit <=NUM_LEDS ){
    for(minbit; minbit <= maxbit; minbit = minbit + 1) {
        leds[minbit] = color;
        }
        if(intensity<=125){
        FastLED.setBrightness(intensity);
        }
        FastLED.show();  
  }
}

void FillLEDsFromPaletteColors( uint8_t colorIndex,uint8_t brightness)
{
    
    for( int i = 0; i < NUM_LEDS; i++) {
        leds[i] = ColorFromPalette( currentPalette, colorIndex, brightness, currentBlending);
        colorIndex += 20;
    }
}

360Wheel_Radio_Light_i2C_ver8.ino

Arduino
Arduino Uno code for 4 DC motor driver controls and Radio signal decoding.
#include <Wire.h>
#include <FastLED.h>

#define NUM_LEDS 60
// Fly Sky Receiver Chn 1-6 in PWM mode
int pinPWM1 = 10;
int pinPWM2 = 11;
int pinPWM3 = 12;
int pinPWM4 = 13;
int pinPWM5 = A0;
int pinPWM6 = A1;

#define lowPWM 611  //611
#define highPWM 1227


// Data pin that led data will be written out over
#define DATA_PIN A2
CRGB leds[NUM_LEDS];

CRGBPalette16 currentPalette;
TBlendType    currentBlending;



#define BRAKE 0
#define CW    1
#define CCW   2
#define HALT  3

//MOTOR 1
#define MOTOR1_DIR 2
#define PWM_MOTOR1 3

//MOTOR 4
#define MOTOR4_DIR 4
#define PWM_MOTOR4 5

//MOTOR 2
#define MOTOR2_DIR 7
#define PWM_MOTOR2 6

//MOTOR 3
#define MOTOR3_DIR 8
#define PWM_MOTOR3 9

#define MOTOR_1 0
#define MOTOR_2 1
#define MOTOR_3 2
#define MOTOR_4 3

#define RGB 12

short Speed = 15;  //default motor speed
short Speed1 = 0;  
short Speed2 = 0;  
short Speed3 = 0;  
short Speed4 = 0;  

unsigned short usMotor_Status = BRAKE;

unsigned long duration1,duration2,duration3,duration4,duration5,duration6;
unsigned long chn1,chn2,chn3,chn4,chn5,chn6;
unsigned long timeout = 50000;

int i2cdata[32];              //i2cdata[0] = index 0 is the register offset. useful for selection and commands
int i2cnumbytes=32;           //number of bytes in i2c
int i=0;
int serial_enable = 0;
int rf_enable = 1;
int i2c_enable = 0;
int readRFchannel5 =0;
int readRF_enable = 0;

void setup()                         
{
  // Fly Sky Radio Receiver //
  pinMode(pinPWM1, INPUT);
  pinMode(pinPWM2, INPUT);
  pinMode(pinPWM3, INPUT);
  pinMode(pinPWM4, INPUT);
  pinMode(pinPWM5, INPUT);
  pinMode(pinPWM6, INPUT);
  
  Wire.begin(0x0A); // Start I2C on Address 0x0A
  Wire.onReceive(receiveEvent); // Receive message from RPI - write to this Slave Arduino
  Wire.onRequest(requestEvent); // Sending information back to Rpi

    FastLED.addLeds<WS2812B, DATA_PIN, GRB>(leds, NUM_LEDS);
    FastLED.setBrightness(120);
    currentPalette = RainbowColors_p;
    FillLEDsFromPaletteColors(0);
    FastLED.show();
 

  setPwmFrequency(3, 1);    //change PWM1 = 31.25kHz
  setPwmFrequency(5, 1);    //change PWM2 = 62.5kHz
  setPwmFrequency(6, 1);    //change PWM3 = 62.5kHz
  setPwmFrequency(9, 1);    //change PWM4 = 62.5kHz
   
  pinMode(MOTOR1_DIR, OUTPUT);
  pinMode(MOTOR2_DIR, OUTPUT);
  pinMode(MOTOR3_DIR, OUTPUT);
  pinMode(MOTOR4_DIR, OUTPUT);

  pinMode(PWM_MOTOR1, OUTPUT);
  pinMode(PWM_MOTOR2, OUTPUT);
  pinMode(PWM_MOTOR3, OUTPUT);
  pinMode(PWM_MOTOR4, OUTPUT);

  Serial.begin(115200);              // Initiates the serial to do the monitoring 
  Serial.println("Begin motor control");
  Serial.println(); //Print function list for user selection
  Serial.println("Enter number for control option:");
  Serial.println("5. STOP");
  Serial.println("8. FORWARD");
  Serial.println("2. REVERSE");
  Serial.println("4. LEFT");
  Serial.println("5. HALT");
  Serial.println("6. RIGHT");
  Serial.println("7. AngleForwardLeft");
  Serial.println("9. AngleForwardRight");
  Serial.println("1. AngleReverseLeft");
  Serial.println("3. AngleReverseRight");
  Serial.println("+. INCREASE SPEED");
  Serial.println("-. DECREASE SPEED");
  Serial.println("/. ROTATECCW");
  Serial.println("*. ROTATECW");
  Serial.println("[. SPINLEFT");
  Serial.println("]. SPINRIGHT");
  Serial.println();



}

void loop() 
{

  duration6 = pulseIn(pinPWM6, HIGH,timeout);
  chn6=map(duration6,lowPWM,highPWM,0,100);
  if(chn6>2500000){
    chn6=0;
  } //allows Chn6 Radio to control whether RF control enable or disable
  if ((chn6>40) && (chn6 <60)) {
    rf_enable=1;
    // Serial.println("RF enable");
  }
  else if(chn6<40){
    rf_enable=0;
    //Serial.println("RF disable");
  }
  
  if(rf_enable==1){
  
  // Fly Sky Radio receiver
  duration1 = pulseIn(pinPWM1, HIGH,timeout);
  chn1=map(duration1,lowPWM,highPWM,0,100);
  //Serial.print(duration1);
  //Serial.print(chn1);
  //Serial.print(",");
  duration2 = pulseIn(pinPWM2, HIGH,timeout);
  chn2=map(duration2,lowPWM,highPWM,0,100);
  //Serial.print(duration2);
  //Serial.print(chn2);
  //Serial.print(",");
  duration3 = pulseIn(pinPWM3, HIGH,timeout);
  chn3=map(duration3,lowPWM,highPWM,0,200);
  //Serial.print(chn3);
  //Serial.print(duration3);
  //Serial.print(",");
  duration4 = pulseIn(pinPWM4, HIGH,timeout);    //1st Joystick left-right
  chn4=map(duration4,lowPWM,highPWM,0,100);
  //Serial.print(chn4);
  //Serial.print(duration4);
  //Serial.print(",");
  duration5 = pulseIn(pinPWM5, HIGH,timeout);
  chn5=map(duration5,lowPWM,highPWM,0,100);
  if(chn5>2500000){
    chn5=0;
  }
  //Serial.print(chn5);
  //Serial.print(duration5);
  //Serial.print(",");
  duration6 = pulseIn(pinPWM6, HIGH,timeout);
  chn6=map(duration6,lowPWM,highPWM,0,100);
  if(chn6>2500000){
    chn6=0;
  }
  //Serial.println(chn6);
  //Serial.println(duration6);
   
  //Move Forward/ Backward radio control 
  if (chn2>52) {
    Speed1=Speed+chn3+chn2-52;
    Speed2=Speed+chn3+chn2-52;
    Speed3=Speed+chn3+chn2-52;
    Speed4=Speed+chn3+chn2-52;
    //Serial.println("Forward");
    usMotor_Status = CW;
    motorGo(MOTOR_1, usMotor_Status, Speed1);
    motorGo(MOTOR_2, usMotor_Status, Speed2);
    motorGo(MOTOR_3, usMotor_Status, Speed3);
    motorGo(MOTOR_4, usMotor_Status, Speed4);
  }

  else if (chn2<48) {
    Speed1=Speed+chn3+48-chn2;
    Speed2=Speed+chn3+48-chn2;
    Speed3=Speed+chn3+48-chn2;
    Speed4=Speed+chn3+48-chn2;
    //Serial.println("Backward");
    usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, Speed1);
    motorGo(MOTOR_2, usMotor_Status, Speed2);
    motorGo(MOTOR_3, usMotor_Status, Speed3);
    motorGo(MOTOR_4, usMotor_Status, Speed4);
  }
 else if (chn2>=48 && chn2 <=52) {
    Speed1=0;
    Speed2=0;
    Speed3=0;
    Speed4=0;
    //Serial.println("Stop");
    //usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, Speed1);
    motorGo(MOTOR_2, usMotor_Status, Speed2);
    motorGo(MOTOR_3, usMotor_Status, Speed3);
    motorGo(MOTOR_4, usMotor_Status, Speed4);
  }

  //Move Left/Right radio control //
     if (chn4>60) {
      Speed1=Speed+chn3+chn4-55;
      Speed2=Speed+chn3+chn4-55;
      Speed3=Speed+chn3+chn4-55;
      Speed4=Speed+chn3+chn4-55;
      //Serial.println("Left");
      usMotor_Status = CW;
      motorGo(MOTOR_1, usMotor_Status, Speed1);
      usMotor_Status = CCW;
      motorGo(MOTOR_2, usMotor_Status, Speed2);
      usMotor_Status = CW;
      motorGo(MOTOR_3, usMotor_Status, Speed3);
      usMotor_Status = CCW;
      motorGo(MOTOR_4, usMotor_Status, Speed4);
    }
    else if (chn4<40) {
      Speed1=Speed+chn3+45-chn4;
      Speed2=Speed+chn3+45-chn4;
      Speed3=Speed+chn3+45-chn4;
      Speed4=Speed+chn3+45-chn4;
      //Serial.println("Right");
      usMotor_Status = CCW;
      motorGo(MOTOR_1, usMotor_Status, Speed1);
      usMotor_Status = CW;
      motorGo(MOTOR_2, usMotor_Status, Speed2);
      usMotor_Status = CCW;
      motorGo(MOTOR_3, usMotor_Status, Speed3);
      usMotor_Status = CW;
      motorGo(MOTOR_4, usMotor_Status, Speed4);
    }
//    else if (chn4>=48 && chn4 <=52) {
//    Speed1=0;
//    Speed2=0;
//    Speed3=0;
//    Speed4=0;
//    Serial.println("Stop");
//    //usMotor_Status = CCW;
//    motorGo(MOTOR_1, usMotor_Status, Speed1);
//    motorGo(MOTOR_2, usMotor_Status, Speed2);
//    motorGo(MOTOR_3, usMotor_Status, Speed3);
//    motorGo(MOTOR_4, usMotor_Status, Speed4);
//  }  
    
    //Spin CW/CCW radio control //
     if (chn1>52) {
      Speed1=Speed+chn3+chn1-52;
      Speed2=Speed+chn3+chn1-52;
      Speed3=Speed+chn3+chn1-52;
      Speed4=Speed+chn3+chn1-52;
      //Serial.println("Rotate CW");
      usMotor_Status = CW;
      motorGo(MOTOR_1, usMotor_Status, Speed1);
      usMotor_Status = CW;
      motorGo(MOTOR_2, usMotor_Status, Speed2);
      usMotor_Status = CCW;
      motorGo(MOTOR_3, usMotor_Status, Speed3);
      usMotor_Status = CCW;
      motorGo(MOTOR_4, usMotor_Status, Speed4);
     }
     else if(chn1<48){
      Speed1=Speed+chn3+48-chn1;
      Speed2=Speed+chn3+48-chn1;
      Speed3=Speed+chn3+48-chn1;
      Speed4=Speed+chn3+48-chn1;
      //Serial.println("Rotate CCW");
      usMotor_Status = CCW;
      motorGo(MOTOR_1, usMotor_Status, Speed1);
      usMotor_Status = CCW;
      motorGo(MOTOR_2, usMotor_Status, Speed2);
      usMotor_Status = CW;
      motorGo(MOTOR_3, usMotor_Status, Speed3);
      usMotor_Status = CW;
      motorGo(MOTOR_4, usMotor_Status, Speed4);
     }
     
  //******************************************************//
  }

  if(readRF_enable==1){
      duration5 = pulseIn(pinPWM5, HIGH,timeout);
      chn5=map(duration5,lowPWM,highPWM,0,100);
      readRFchannel5=chn5;
      //Serial.println("Channel5");
      readRF_enable=0;
  }
  
  if(serial_enable==1)
  {
  //// serial data read from Python /////
  char user_input;   
  while(Serial.available())
  {
    user_input = Serial.read(); //Read user input and trigger appropriate function
    
    if (user_input =='5')
    {
      Stop();
      //Halt();
    }
    else if(user_input =='8')
    {
      Forward();
      
    }
    else if(user_input =='2')
    {
      Reverse();
    }
    else if(user_input =='4')
    {
      Left();
    }
    else if(user_input =='6')
    {
      Right();
    }
    else if(user_input =='/')
    {
      RotateCW();
    }
    else if(user_input =='*')
    {
      RotateCCW();
    }
    else if(user_input =='7')
    {
      AngleForwardLeft();
    }
    else if(user_input =='9')
    {
      AngleForwardRight();
    }
    else if(user_input =='1')
    {
      AngleReverseLeft();
    }
    else if(user_input =='3')
    {
      AngleReverseRight();
    }
    else if(user_input =='+')
    {
      IncreaseSpeed();
    }
    else if(user_input =='-')
    {
      DecreaseSpeed();
    }
    else if(user_input =='[')
    {
      ForwardSpinLeft();
    }
    else if(user_input ==']')
    {
      ForwardSpinRight();
    }
    else if(user_input =='o')
    {
      ReverseSpinLeft();
    }
    else if(user_input =='p')
    {
      ReverseSpinRight();
    }
    else if(user_input =='5')
    {
      Halt();
    }
    else if(user_input =='g')
    {
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led] = CRGB::Green;
      FastLED.show();  
      }
    } 
    else if(user_input =='r')
    {
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led] = CRGB::Red;
      FastLED.show();  
      }
    } 
    else if(user_input =='b')
    {
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led] = CRGB::Blue;
      FastLED.show();  
      }
    }     
    else if(user_input =='w')
    {
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led] = CRGB::White;
      FastLED.show();  
      }
    }     
    else if(user_input =='x')
    {
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led] = CRGB::Black;
      FastLED.show();  
      }
    }     
    else if(user_input =='t')
    {
      FastLED.setBrightness(255);
      for(int Led = 0; Led < 60; Led = Led + 1) {
      leds[Led+1] = CRGB::White;
      leds[Led] = CRGB::Black;       
      FastLED.show();
      }
      FastLED.setBrightness(120);
    }     
    
      else if(user_input =='u')
      {
      FastLED.setBrightness(255);
      }     
      else if(user_input =='j')
      {
      FastLED.setBrightness(120);
      }     
        
    else
      {
      //Serial.println("Invalid option entered.");
      }
      
  }
  }

 if(i2c_enable==1) {
   Serial.println("I2c enable");
  /// i2c data read from raspberry Pi ///
          if (i2cdata[1] ==5)
          {
          Stopi(i2cdata[2]);
          }
        else if(i2cdata[1] ==2)
          {
          Forwardi(i2cdata[2]);
          }
        else if(i2cdata[1] ==8)
          {
          Reversei(i2cdata[2]);
          }
        else if(i2cdata[1] ==10)
          {
          Lefti(i2cdata[2]);
          }
        else if(i2cdata[1] ==11)
          {
          Righti(i2cdata[2]);
          }
        else if(i2cdata[1] ==4)
          {
          RotateCWi(i2cdata[2]);
          }
        else if(i2cdata[1] ==6)
          {
          RotateCCWi(i2cdata[2]);
          }
        else if(i2cdata[1] ==7)
          {
          AngleForwardLeft();
          }
        else if(i2cdata[1] ==9)
          {
          AngleForwardRight();
          }
        else if(i2cdata[1] ==1)
          {
          AngleReverseLeft();
          }
        else if(i2cdata[1] ==3)
          {
          AngleReverseRight();
          }
        else if(i2cdata[1] ==13)
          {
          IncreaseSpeed();
          //i2cdata[1]=0;
          }
        else if(i2cdata[1] ==12)
          {
          DecreaseSpeed();
          //i2cdata[1]=0;
          }
        else if(i2cdata[1] ==14)
          {
          Halt();
          }
        else if(i2cdata[1] ==15)
          {
          ForwardSpinLeft();
          }
        else if(i2cdata[1] ==16)
          {
          ForwardSpinRight();
          }
        else if(i2cdata[1] ==17)
          {
          ReverseSpinLeft();
          }
        else if(i2cdata[1] ==18)
          {
          ReverseSpinRight();
          }
        else if(i2cdata[1] ==50)
          {
            for(int Led = 0; Led < 60; Led = Led + 1) {
            leds[Led] = CRGB::Green;
            FastLED.show();  
            }
          } 
        else if(i2cdata[1] ==51)
          {
           for(int Led = 0; Led < 60; Led = Led + 1) {
           leds[Led] = CRGB::Red;
           FastLED.show();  
           }
          }  
        else if(i2cdata[1] ==52)
          {
            for(int Led = 0; Led < 60; Led = Led + 1) {
            leds[Led] = CRGB::Blue;
            FastLED.show();  
            }
          }     
        else if(i2cdata[1] ==53)
          {
            for(int Led = 0; Led < 60; Led = Led + 1) {
            leds[Led] = CRGB::White;
            FastLED.show();  
             }
          }     
        else if(i2cdata[1] ==54)
          {
            for(int Led = 0; Led < 60; Led = Led + 1) {
            leds[Led] = CRGB::Black;
            FastLED.show();  
             }
          }     
        else if(i2cdata[1] ==55)
          {
            FastLED.setBrightness(255);
            for(int Led = 0; Led < 60; Led = Led + 1) {
            leds[Led+1] = CRGB::White;
            leds[Led] = CRGB::Black;  
            FastLED.show();  
             }
           FastLED.setBrightness(120);
          }     
        else if(i2cdata[1] ==60)
          {
          FastLED.setBrightness(120);
          }
        else if(i2cdata[1] ==61)
          {
          FastLED.setBrightness(255);
          }      
        
        else
        {
         //Halt();
        }
  i2c_enable=0;
 }



}



//////////////////////////// Functions /////////////////////////////
void Stop()
{
  int i;
  int tempspeed;
  Serial.println("Stop");
  tempspeed=Speed;
  usMotor_Status = BRAKE;
  for(i=0;i<=200;i++){
    if(tempspeed>0) {
      tempspeed = tempspeed-1;
      delay(10);
    }
    else {
      tempspeed = 0;
      i=200;
    }
    if(Speed1!=0) {
      motorGo(MOTOR_1, usMotor_Status, tempspeed);        
    }
    if(Speed2!=0) {
      motorGo(MOTOR_2, usMotor_Status, tempspeed);        
    }
    if(Speed3!=0) {
      motorGo(MOTOR_3, usMotor_Status, tempspeed);        
    }
    if(Speed4!=0) {
      motorGo(MOTOR_4, usMotor_Status, tempspeed);        
    }

  }

  
  Speed1=0;
  Speed2=0;
  Speed3=0;
  Speed4=0;

}

void Stopi(int deceleration)
{
  int i;
  int tempspeed;
  Serial.println("Stopi:");
  tempspeed=Speed;
  usMotor_Status = BRAKE;
  for(i=0;i<=Speed;i++){
    if(tempspeed>0) {
      tempspeed = tempspeed-1;
      delay(deceleration*10);
      }
   else {
      tempspeed = 0;
      i=Speed;
    }
   // Serial.print(deceleration);
   // Serial.print(",");
   // Serial.println(tempspeed);
    
    if(Speed1!=0) {
      motorGo(MOTOR_1, usMotor_Status, tempspeed);        
    }
    if(Speed2!=0) {
      motorGo(MOTOR_2, usMotor_Status, tempspeed);        
    }
    if(Speed3!=0) {
      motorGo(MOTOR_3, usMotor_Status, tempspeed);        
    }
    if(Speed4!=0) {
      motorGo(MOTOR_4, usMotor_Status, tempspeed);        
    }

  }

  
  Speed1=0;
  Speed2=0;
  Speed3=0;
  Speed4=0;

}

void Halt()
{
  Serial.println("Halt");
  usMotor_Status = HALT;
  motorGo(MOTOR_1, usMotor_Status, 0);
  motorGo(MOTOR_2, usMotor_Status, 0);
  motorGo(MOTOR_3, usMotor_Status, 0);
  motorGo(MOTOR_4, usMotor_Status, 0);
  Speed1=0;
  Speed2=0;
  Speed3=0;
  Speed4=0;
}

void Forward()
{
  
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Forward");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void Forwardi(int motorspeed)
{
  
  Speed=motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.print("Forward:");
  Serial.println(Speed);
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  motorGo(MOTOR_4, usMotor_Status, Speed4);
  
}

void Reverse()
{
  
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Reverse");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void Reversei(int motorspeed)
{
  
  Speed=motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.print("Reverse:");
  Serial.println(Speed);
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void Left()
{
  
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Left");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void Lefti(int motorspeed)
{
  Speed=motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Left");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void Right()
{
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Right");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CCW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CCW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}
void Righti(int motorspeed)
{
  Speed=motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Right");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CCW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CCW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void RotateCW()
{
  
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Rotate CW");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CCW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void RotateCWi(int motorspeed)
{
  Speed = motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Rotate CW");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CCW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}
void RotateCCW()
{
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Rotate CCW");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CCW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void RotateCCWi(int motorspeed)
{
  Speed = motorspeed;
  Speed1=Speed;
  Speed2=Speed;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("Rotate CCW");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CCW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void AngleReverseLeft()
{
  Speed1=Speed;
  Speed2=0;
  Speed3=Speed;
  Speed4=0;
  Serial.println("AngleReverseLeft");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void AngleReverseRight()
{
  Speed1=0;
  Speed2=Speed;
  Speed3=0;
  Speed4=Speed;
  Serial.println("AngleReverseRight");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CCW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CCW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}
void AngleForwardLeft()
{
  Speed1=0;
  Speed2=Speed;
  Speed3=0;
  Speed4=Speed;
  Serial.println("AngleForwardLeft");
  usMotor_Status = CCW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CCW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}
void AngleForwardRight()
{
  Speed1=Speed;
  Speed2=0;
  Speed3=Speed;
  Speed4=0;

  Serial.println("AngleForwardRight");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  usMotor_Status = CW;
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  usMotor_Status = CW;
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  usMotor_Status = CW;
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void ForwardSpinLeft()
{
  
  Speed1=Speed;
  Speed2=Speed;
  Speed3=0;
  Speed4=0;
  Serial.println("ForwardSpinLeft");
  usMotor_Status = CW;
  motorGo(MOTOR_1, usMotor_Status, Speed1);
  motorGo(MOTOR_2, usMotor_Status, Speed2);
  motorGo(MOTOR_3, usMotor_Status, Speed3);
  motorGo(MOTOR_4, usMotor_Status, Speed4);
}

void ForwardSpinRight()
{
  
  Speed1=0;
  Speed2=0;
  Speed3=Speed;
  Speed4=Speed;
  Serial.println("ForwardSpinRight");
  usMotor_Status = CW;
...

This file has been truncated, please download it to see its full contents.

i2c_motorcontrol.py

Python
Use smbus to control motor's speed, duration. Also read Radio switches whether to override between autonomous or manual control. Return Radio switches
from smbus2 import SMBus
import time
address  = 0x0A
read_on  = 1
read_off = 0
write_reg = 0  # 0 = i2c writing is disable for motor control
               # 1 = i2c writing is enable for motor control
               # 2 = rf is disable for motor control
               # 3 = rf is enable for motor control


def motor_read_block(regread = read_on,num=10):
    with SMBus(1) as bus:
        # Read a block of 16 bytes from address 80, offset 0
        block = bus.read_i2c_block_data(address, regread, num)
        # Returned value is a list of 16 bytes
        #print(block)
        return(block,num)

def motor_write_block(regwrite = 0,data = [0,1,2,3,4]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
        bus.write_i2c_block_data(address, regwrite, data)

def rf_disable(write_reg = 2):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [44])
    time.sleep(0.01)

def rf_enable(write_reg = 3):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [88])
    time.sleep(0.01)

def motor_forward(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [8,speed])
    time.sleep(waitime)

def motor_reverse(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [2,speed])
    time.sleep(waitime)

def motor_stop(deceleration = 0,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [5,deceleration])
        if(deceleration>100):
            time.sleep(2)
        elif ((deceleration>10) & (deceleration<100)):
            time.sleep(1)
        else:
            time.sleep(0)

def motor_left(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [4,speed])
    time.sleep(waitime)

def motor_right(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [6,speed])
    time.sleep(waitime)

def motor_cw(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [10,speed])
    time.sleep(waitime)

def motor_ccw(speed = 10,waitime=0.1,write_reg = 1):
    with SMBus(1) as bus:
        bus.write_i2c_block_data(address, write_reg, [11,speed])
    time.sleep(waitime)

rf_disable()            #disable Radio before run control from RPI.

i2cread_ultrasonic.py

Python
Use smbus to read ultrasonic proximity sensing data. Return proximity sensing data for obstacle avoidance
from smbus2 import SMBus

address  = 0x8
read_on  = 1
read_off = 0
write_reg = 0

def ultrasonic_read(regread = read_on,num=4):
    with SMBus(1) as bus:
        # Read a block of 16 bytes from address 80, offset 0
        block = bus.read_i2c_block_data(address, regread, num)
        # Returned value is a list of 16 bytes
        #print(block)
        return(block,num)


def ultrasonic_write(write_reg = 0,data = [0,1,2,3,4]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, write_reg, data)

#while 1:
#    data,num = ultrasonic_read()
#    print(data)

i2c_robotarm.py

Python
Use smbus to control linear servo , angular servo and mist activitation. Return servo position
from smbus2 import SMBus
import time
address  = 0x9  
read_on  = 1
read_off = 0
write_reg = 4       # 4 = linear arm and rotary servo accessible for writing
                    # 5 = linear arm only rotary servo accessible for writing
                    # 6 = linear arm homing
                    # 7 = linear arm encodercount read only

def linear_arm_servo(position = 1700,angle = 0):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          lower = position & 0x00FF
          upper = position >> 8
          bus.write_i2c_block_data(address, 4, [lower,upper,angle])
          time.sleep(0.05)

def arm_servo(angle = 0):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, 5, [angle])
          time.sleep(0.05)

def home_arm(angle = 0):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, 6, [angle])
          print("Homing ... 5 secs")
          time.sleep(5)

def read_linear_position():
    with SMBus(1) as bus:
        # Read a block of 16 bytes from address 80, offset 0
        block = bus.read_i2c_block_data(address, 7, 3)
        # Returned value is a list of 16 bytes
        position = block[0] + (block[1]<<8)
        servo = block[2]
        print(position,servo)
        return(position,servo)
        time.sleep(0.5)

#home_arm()
#while True:
#        read_linear_position()
#        arm_servo(5)
#        time.sleep(1)
#        arm_servo(95)
#        time.sleep(1)
#        arm_servo(180)
#       time.sleep(1)
#        linear_arm_servo(3000,95)
#        time.sleep(1)
#        linear_arm_servo(300,5)
#        time.sleep(1)
#        linear_arm_servo(1700,180)
#       time.sleep(1)

i2c_lightpump.py

Python
Use smbus to control LED brightness and water pump flow rate. Return nil.
from smbus2 import SMBus
import time
address  = 0x8
read_on  = 1
read_off = 0
write_reg = 2       # 2 = ledpwm accessible for writing
                    # 3 = rgbled accessible for writing

def led(data = [10]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, write_reg, data)
          time.sleep(0.01)

def rgbled(data = [1]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, 3, data)
          time.sleep(0.01)

def motorpump_on(data = [130]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, 4, data)
          time.sleep(0.1)

def motorpump_off(data = [0]):
    with SMBus(1) as bus:
        # Write a byte to address 80, offset 0
          bus.write_i2c_block_data(address, 4, data)
          time.sleep(0.1)


#motorpump_on()
#time.sleep(1)
#motorpump_off()
#led([55])
#time.sleep(1)
#rgbled([2])
#time.sleep(0.5)
#led([0])
#rgbled([4])

motiondetection5.py

Python
Use to launch the robot upon motion detection activated. Return successful flag of detected movement.
import numpy as np
import cv2
import time
import imutils

graphice_enable = 0
screen_width = 500  # 720 #640 #500
screen_height = 281  # 405 #360 #281
img = 0
initframe =0
ret = 0
cap = 0
img2= 0
frame = 0
delta = 0
current_time = 0
delta_time = 0
init_time = 0
status = ""
x = 0
y = 0
h = 0
w = 0
area =0

def camerasetup():
    global initframe,ret, cap,init_time,img0
    print("Start Motion Detection")
    cap = cv2.VideoCapture(0)
    
    for i in range(10):
        ret, img0 = cap.read()
        if ret is None :
            print("No Camera")
            break
    time.sleep(0.1)
    img0 = cv2.resize(img0, (screen_width, screen_height))
    initframe = cv2.cvtColor(img0, cv2.COLOR_BGR2GRAY)
    init_time = time.time()


def motion(graphic_enable=0,refresh=30):
    global img,img2, frame, delta, initframe,status
    global current_time, delta_time, init_time
    global x,y,w,h, area

    font = cv2.FONT_HERSHEY_SIMPLEX
    bottomLeftCornerOfText = (10, 500)
    fontScale = 0.8
    fontColor = (255, 125, 125)
    lineType = 2

    for i in range(1):
        ret, img = cap.read()                               #video stream read
        if ret is None :
            print("No Camera")
            break

        img = cv2.resize(img, (screen_width, screen_height))
        img2=img
        frame = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)       # convert to grayscale for easier calculation
        delta = cv2.absdiff(initframe, frame)               #subtraction absolute first frame and current frame

        delta = cv2.dilate(delta, None, iterations=2)
        delta = cv2.threshold(delta, 50, 255, cv2.THRESH_BINARY)[1]     #making higher differention pixels for contour.

        contours, hierarchy = cv2.findContours(delta,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
        cnts = cv2.findContours(delta.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)

     #   cv2.drawContours(img2, contours, -1, (0, 255, 255), 1)
     #       if (len(contours)==0):
      #      status = "X"
       #     x = 0
        #    y = 0
         #   h = 0
          #  w = 0
          #  area = 0

        for c in contours:
            if cv2.contourArea(c) > 200:
                #print(cv2.contourArea(c))
                area=cv2.contourArea(c)
                (x, y, w, h) = cv2.boundingRect(c)
                cv2.rectangle(img2, (x, y), (x + w, y + h), (0, 255, 0), 2)
                text = "Motion"
               # cv2.putText(img2, "Area Status: {}".format(text), (10, 50),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
                status = text
            else:
                status = "X"
                area =0
                x = 0
                y = 0
                h = 0
                w = 0
                continue

        current_time = time.time()
        delta_time = current_time - init_time
        if delta_time > refresh:                #refresh the initframe after certain duration
            ret, initframe = cap.read()
            initframe = cv2.resize(initframe, (screen_width, screen_height))
            initframe = cv2.cvtColor(initframe, cv2.COLOR_BGR2GRAY)
            init_time = time.time()
            #print("Clear Image")

        return (area,status, x, y, h, w)

def closecamera():
    cap.release()
    cv2.destroyAllWindows()

def loopautodetect():
    camerasetup()
    while True:
        detect=motion(1,2)[0]
        if(detect>0):
            print("Detect",detect)
            break

    closecamera()


if (__name__ == "__main__"):

    camerasetup()
    while True:
        detect=motion(1,2)[0]
        if(detect>0):
            print("detect",detect)
       # cv2.imshow("Init Frame", img0)
        #cv2.imshow('Current Real Frame', img)
       # cv2.imshow('Delta Frame', delta)
       # cv2.imshow('Delta Countour', img2)

        k=cv2.waitKey(1)
        if k == ord('x'):       # exit
            break
        elif k == ord('r'):     #manual refresh initframe
            ret, initframe = cap.read()
            initframe = cv2.resize(initframe, (screen_width, screen_height))
            initframe = cv2.cvtColor(initframe, cv2.COLOR_BGR2GRAY)
            print("Manual Refresh")
    closecamera()

main_autonomousB.py

Python
Fully Autonomous 6 Stages for Sanitizing on Demand on Lift Buttons. Can be override with radio control
#--------------------------------------------#
#           Main Program for Robot           #
#          Written by Yong Voon Siew         #
#                 7 May 2020                 #
#--------------------------------------------#

#import audioos as aud
import colordetectionseek as cd
import colorseqdetect9 as cs
import i2cread_ultrasonic as us 
import i2c_motorcontrol as mot
import i2c_lightpump as led
import i2c_robotarm as arm
import buzzer as bz
import time
import motiondetection5 as md
import navigationlocation5 as nv

print("Start AutonomousB")
bz.alarm()

def autonomousA_stage1():           #autonomous locate Color
    
    led.rgbled([1])
    window = 20
    detectionwidth=20
    detectionarea = 4500
    mot.rf_disable()            #disable Radio before run control from RPI.
    cd.camera_setup()
    led.led([45])
    time.sleep(0.2)
    led.led([1])
    time.sleep(0.2)
    led.led([200])
    print("Hello Autonomous!")
    while 1:
        x,y,w,h = cd.camera_search(cd.graphic_on)
        k=cs.cv2.waitKey(1) 
        if k == ord('q'):
            break
        print("x=",x,"y",y,"w=",w,"h=",h,"area",h*w)
            
        if(w>10):
            if(x>cd.screen_width/2+window) :
                mot.motor_cw(20,0)
            elif ((x<cd.screen_width/2-window) & (x!=0)):
                mot.motor_ccw(20,0)
            elif((x>cd.screen_width/2-window) & (x<cd.screen_width/2+window)):
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                if((ultrasnc_data[1]>=15) and (ultrasnc_data[0]>=15)):
                    if(ultrasnc_data[2]<15):
                        print("forward rightside")
                        mot.motor_ccw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    elif(ultrasnc_data[3]<15):
                        print("forward leftside")
                        mot.motor_cw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    else: 
                        print("Forward straight")
                        mot.motor_forward(40,0.3)

                elif ((ultrasnc_data[1]<15) and (ultrasnc_data[2]>15) and ultrasnc_data[0]>15 ):
                    print("left")
                    mot.motor_left(45,0.1) 
                elif((ultrasnc_data[0]<15) and (ultrasnc_data[3]>15) and ultrasnc_data[1]>15):
                    print("right")
                    mot.motor_right(45,0.1) 
                elif ((ultrasnc_data[0]<15) and (ultrasnc_data[1]<15)):
                    print("reverse")    
                    mot.motor_reverse(50,0.1)
                            
            else:
                mot.motor_stop(0)
                
            if((h*w)>detectionarea):
                mot.motor_stop(0)
                #led.rgbled([3])
                break
  
    cd.camera_close()
    print("\n Done. Mission Accomplished!\n")
    mot.motor_stop(10)

def autonomousA_stage2(detectionarea=17000):           #autonomous locate Color Sequencing
    led.rgbled([1])
    window = 20
    detectionwidth=20
    detectionarea = detectionarea
    mot.rf_disable()            #disable Radio before run control from RPI.
    cs.camera_setup()
    led.led([45])
    time.sleep(0.2)
    led.led([1])
    time.sleep(0.2)
    led.led([200])
    print("Hello Autonomous!")
    while 1:
        #x,y,w,h = cd.camera_search(cd.graphic_off)
        success,x,y,w,h,area = cs.colorsequence(cs.graphic_on)
        k=cs.cv2.waitKey(1) 
        if k == ord('q'):
            break
        print("success=",success,"x=",x,"y",y,"area=",area,"w=",w,"h=",h)
            
        if(success):
            if(x>cs.screen_width/2+window) :
                mot.motor_cw(20,0)
            elif ((x<cs.screen_width/2-window) & (x!=0)):
                mot.motor_ccw(20,0)
            elif((x>cs.screen_width/2-window) & (x<cs.screen_width/2+window)):
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                if((ultrasnc_data[1]>=15) and (ultrasnc_data[0]>=15)):
                    if(ultrasnc_data[2]<15):
                        print("forward rightside")
                        mot.motor_ccw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    elif(ultrasnc_data[3]<15):
                        print("forward leftside")
                        mot.motor_cw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    else: 
                        print("Forward straight")
                        mot.motor_forward(40,0.3)

                elif ((ultrasnc_data[1]<15) and (ultrasnc_data[2]>15) and ultrasnc_data[0]>15 ):
                    print("left")
                    mot.motor_left(45,0.1) 
                elif((ultrasnc_data[0]<15) and (ultrasnc_data[3]>15) and ultrasnc_data[1]>15):
                    print("right")
                    mot.motor_right(45,0.1) 
                elif ((ultrasnc_data[0]<15) and (ultrasnc_data[1]<15)):
                    print("reverse")    
                    mot.motor_reverse(50,0.1)
                            
            else:
                mot.motor_stop(0)
                
            if(area>detectionarea):
                mot.motor_stop(100)
                #led.rgbled([3])
                break
  
    cs.camera_close()
    print("\n Done. Mission Accomplished!\n")
   # wallsquaring(15)            # if during final detection from camera it stop at an angle it will align perpendicular to the wall using 2 front ultrasonic.
    time.sleep(0.5)
    mot.motor_left(45,1) 
    mot.motor_right(45,2) 
    mot.motor_stop(50) 
    time.sleep(0.5)
    mot.motor_stop(10)
    

def wallsquaring(distance = 15):
    ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
    print(ultrasnc_data)
    for n in range(15):
        ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
        print(ultrasnc_data)
        if(ultrasnc_data[0]!=ultrasnc_data[1] and ultrasnc_data[0]<=40 and ultrasnc_data[1]<=40):
            for i in range(500):    # align left side 
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                print("Ultrasonic Left:",ultrasnc_data,num)
                time.sleep(0.1)
                if(ultrasnc_data[0]>distance):
                    mot.motor_fwdspinleft(35,0.01)
                elif(ultrasnc_data[0]<(distance-1)):
                    mot.motor_rwdspinleft(35,0.01)
                else:
                    mot.motor_stop(0)
                    break
            for i in range(500):    # align right side
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                print("Ultrasonic Right:",ultrasnc_data,num)
                time.sleep(0.1)
                if(ultrasnc_data[1]>distance):
                    mot.motor_fwdspinright(35,0.01)
                elif(ultrasnc_data[1]<(distance-1)):
                    mot.motor_rwdspinright(35,0.01)
                else:
                    mot.motor_stop(0)
                    break
        elif ((ultrasnc_data[0]>40) or (ultrasnc_data[1]>40)):
            print("Ilegal wall squaring. Too far from any wall.")
            break



def autonomousB_stage1_loop():            #autonomous locate QR Code
    nv.camera_setup()
    while 1:
        nv.readqr(1)
        k=nv.cv2.waitKey(1) 
        if k == ord('x'):
            nv.cv2.destroyWindow("QRCode")
            break
        elif k != -1 and k!=ord('s'):
            print(chr(k))
            nv.charsearch(k)
            #xyz=np.where(barray==ord('4'))
            #print(xyz)
            #print(barray) 
        elif k == ord('s'):
            print("Your character 's'")
            #nv.charsearchdata()
            nv.charsearch(ord('4'))
    nv.camera_close()

def autonomousB_stage1(detectionarea=22500):
    nv.camera_setup()
    led.rgbled([1])
    window = 20
    detectionwidth=20
    detectionarea = detectionarea
    mot.rf_disable()            #disable Radio before run control from RPI.
    led.led([45])
    time.sleep(0.2)
    led.led([1])
    time.sleep(0.2)
    led.led([200])
    print("Autonomous QRCode Detect!")
    while 1:
        x,y,text,h,w = nv.readqr(1)
        area= h*w
        k=nv.cv2.waitKey(1) 
        if k == ord('q'):
            break
        print("x=",x,"y",y,"Text",text,"h",h,"w",w,"area",area)
            
        if(area>100):
            if(x>nv.screen_width/2+window) :
                mot.motor_cw(20,0)
            elif ((x<nv.screen_width/2-window) & (x!=0)):
                mot.motor_ccw(20,0)
            elif((x>nv.screen_width/2-window) & (x<nv.screen_width/2+window)):
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                if((ultrasnc_data[1]>=15) and (ultrasnc_data[0]>=15)):
                    if(ultrasnc_data[2]<15):
                        print("forward rightside")
                        mot.motor_ccw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    elif(ultrasnc_data[3]<15):
                        print("forward leftside")
                        mot.motor_cw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    else: 
                        print("Forward straight")
                        mot.motor_forward(40,0.3)

                elif ((ultrasnc_data[1]<15) and (ultrasnc_data[2]>15) and ultrasnc_data[0]>15 ):
                    print("left")
                    mot.motor_left(45,0.1) 
                elif((ultrasnc_data[0]<15) and (ultrasnc_data[3]>15) and ultrasnc_data[1]>15):
                    print("right")
                    mot.motor_right(45,0.1) 
                elif ((ultrasnc_data[0]<15) and (ultrasnc_data[1]<15)):
                    print("reverse")    
                    mot.motor_reverse(50,0.1)
                            
            else:
                mot.motor_stop(0)
                
            if(area > detectionarea):
                mot.motor_stop(100)
                break
    nv.camera_close()
    print("\n Done. Mission Accomplished!\n")

if(__name__=='__main__'):
    autonomousB_stage1()
    nv.charsearch(ord('4'))
    while True:
        True

if(__name__=='__main__'):

    md.loopautodetect()          #motion detection
    bz.sounder(1)
    print("Movement detected. Start Cleaning!")
    autonomous1_stage1()         #color detection
    bz.sounder(1)
    autonomous1_stage2(10000)    #color sequence detection
    bz.sounder(1)
    autonomous2_stage1()        #qrcode detection and readout
    bz.sounder(1)
    mot.motor_forward(30,2)
    mot.motor_stop(100)
    wallsquaring(20)             #wall squaring
    nv.charsearch(ord('7'))
    arm.linear_arm_servo(50)      #move arm
    print("Activate Arm")
    arm.linear_servo(90)

main_autonomousA.py

Python
Fully Autonomous 5 Stages for Sanitizing on Demand on Door Knobs. Can be override with radio control
#--------------------------------------------#
#           Main Program for Robot           #
#          Written by Yong Voon Siew         #
#                 7 May 2020                 #
#--------------------------------------------#

#import audioos as aud
import colordetectionseek as cd
import colorseqdetect9 as cs
import i2cread_ultrasonic as us 
import i2c_motorcontrol as mot
import i2c_lightpump as led
import i2c_robotarm as arm
import buzzer as bz
import time
import motiondetection5 as md

print("Start")
bz.alarm()

def autonomous1_stage1():
    
    led.rgbled([1])
    window = 20
    detectionwidth=20
    detectionarea = 4500
    mot.rf_disable()            #disable Radio before run control from RPI.
    cd.camera_setup()
    led.led([45])
    time.sleep(0.2)
    led.led([1])
    time.sleep(0.2)
    led.led([200])
    print("Hello Autonomous!")
    while 1:
        x,y,w,h = cd.camera_search(cd.graphic_on)
        k=cs.cv2.waitKey(1) 
        if k == ord('q'):
            break
        print("x=",x,"y",y,"w=",w,"h=",h,"area",h*w)
            
        if(w>10):
            if(x>cd.screen_width/2+window) :
                mot.motor_cw(20,0)
            elif ((x<cd.screen_width/2-window) & (x!=0)):
                mot.motor_ccw(20,0)
            elif((x>cd.screen_width/2-window) & (x<cd.screen_width/2+window)):
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                if((ultrasnc_data[1]>=15) and (ultrasnc_data[0]>=15)):
                    if(ultrasnc_data[2]<15):
                        print("forward rightside")
                        mot.motor_ccw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    elif(ultrasnc_data[3]<15):
                        print("forward leftside")
                        mot.motor_cw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    else: 
                        print("Forward straight")
                        mot.motor_forward(40,0.3)

                elif ((ultrasnc_data[1]<15) and (ultrasnc_data[2]>15) and ultrasnc_data[0]>15 ):
                    print("left")
                    mot.motor_left(45,0.1) 
                elif((ultrasnc_data[0]<15) and (ultrasnc_data[3]>15) and ultrasnc_data[1]>15):
                    print("right")
                    mot.motor_right(45,0.1) 
                elif ((ultrasnc_data[0]<15) and (ultrasnc_data[1]<15)):
                    print("reverse")    
                    mot.motor_reverse(50,0.1)
                            
            else:
                mot.motor_stop(0)
                
            if((h*w)>detectionarea):
                mot.motor_stop(0)
                #led.rgbled([3])
                break
  
    cd.camera_close()
    print("\n Done. Mission Accomplished!\n")
    mot.motor_stop(10)

def autonomous1_stage2():
    
    led.rgbled([1])
    window = 20
    detectionwidth=20
    detectionarea = 17000
    mot.rf_disable()            #disable Radio before run control from RPI.
    cs.camera_setup()
    led.led([45])
    time.sleep(0.2)
    led.led([1])
    time.sleep(0.2)
    led.led([200])
    print("Hello Autonomous!")
    while 1:
        #x,y,w,h = cd.camera_search(cd.graphic_off)
        success,x,y,w,h,area = cs.colorsequence(cs.graphic_on)
        k=cs.cv2.waitKey(1) 
        if k == ord('q'):
            break
        print("success=",success,"x=",x,"y",y,"area=",area,"w=",w,"h=",h)
            
        if(success):
            if(x>cs.screen_width/2+window) :
                mot.motor_cw(20,0)
            elif ((x<cs.screen_width/2-window) & (x!=0)):
                mot.motor_ccw(20,0)
            elif((x>cs.screen_width/2-window) & (x<cs.screen_width/2+window)):
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                if((ultrasnc_data[1]>=15) and (ultrasnc_data[0]>=15)):
                    if(ultrasnc_data[2]<15):
                        print("forward rightside")
                        mot.motor_ccw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    elif(ultrasnc_data[3]<15):
                        print("forward leftside")
                        mot.motor_cw(45,0.1)    
                        mot.motor_forward(40,0.1)
                    else: 
                        print("Forward straight")
                        mot.motor_forward(40,0.3)

                elif ((ultrasnc_data[1]<15) and (ultrasnc_data[2]>15) and ultrasnc_data[0]>15 ):
                    print("left")
                    mot.motor_left(45,0.1) 
                elif((ultrasnc_data[0]<15) and (ultrasnc_data[3]>15) and ultrasnc_data[1]>15):
                    print("right")
                    mot.motor_right(45,0.1) 
                elif ((ultrasnc_data[0]<15) and (ultrasnc_data[1]<15)):
                    print("reverse")    
                    mot.motor_reverse(50,0.1)
                            
            else:
                mot.motor_stop(0)
                
            if(area>detectionarea):
                mot.motor_stop(100)
                #led.rgbled([3])
                break
  
    cs.camera_close()
    print("\n Done. Mission Accomplished!\n")
   # wallsquaring(15)            # if during final detection from camera it stop at an angle it will align perpendicular to the wall using 2 front ultrasonic.
    time.sleep(0.5)
    mot.motor_left(45,1) 
    mot.motor_right(45,2) 
    mot.motor_stop(50) 
    time.sleep(0.5)
    mot.motor_stop(10)
    

def wallsquaring(distance = 15):
    ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
    print(ultrasnc_data)
  

    for n in range(15):
        ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
        print(ultrasnc_data)
        if(ultrasnc_data[0]!=ultrasnc_data[1] and ultrasnc_data[0]<=40 and ultrasnc_data[1]<=40):
            for i in range(500):    # align left side 
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                print("Ultrasonic Left:",ultrasnc_data,num)
                time.sleep(0.1)
                if(ultrasnc_data[0]>distance):
                    mot.motor_fwdspinleft(35,0.01)
                elif(ultrasnc_data[0]<(distance-1)):
                    mot.motor_rwdspinleft(35,0.01)
                else:
                    mot.motor_stop(0)
                    break
            for i in range(500):    # align right side
                ultrasnc_data,num = us.ultrasonic_read(us.read_on,4)
                print("Ultrasonic Right:",ultrasnc_data,num)
                time.sleep(0.1)
                if(ultrasnc_data[1]>distance):
                    mot.motor_fwdspinright(35,0.01)
                elif(ultrasnc_data[1]<(distance-1)):
                    mot.motor_rwdspinright(35,0.01)
                else:
                    mot.motor_stop(0)
                    break
        elif ((ultrasnc_data[0]>40) or (ultrasnc_data[1]>40)):
            print("Ilegal wall squaring. Too far from any wall.")
            break


if(__name__=='__main__'):

    md.loopautodetect()
    bz.sounder(1)
    print("Movement detected. Start Cleaning!")
    autonomous1_stage1()
    bz.sounder(1)
    autonomous1_stage2()
    bz.sounder(1)
    mot.motor_forward(30,2)
    mot.motor_stop(100)
    wallsquaring(15)
    arm.mist()
    arm.mist()
    arm.pump(1,15)
    for i in range(1,10):
        arm.linear_arm_servo(50)
        bz.sounder(0.1)
        time.sleep(0.4)
        arm.linear_arm_servo(1000)
        time.sleep(0.5)
        print(i)
    arm.pump(0,0)
    arm.linear_arm_servo(100)
    arm.mist()
    arm.pump(0,0)

colorseqdetect9.py

Python
Use Color and Color Sequence for Far and Near image for navigation. Return x, y coordinates including area, height and width of the detected image position
import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
from imutils.video import FPS

graphic_off = 0
graphic_on = 1
screen_width = 500 #720 #640 #500
screen_hight = 281 #405 #360 #281
whatcolor = 0
pink = 1
yellow =2
green =3 
blue = 4
orange = 5
graphic_enable = 0
lowerBound = np.array([12, 192, 174])
upperBound = np.array([25, 255, 255])
area = 0

#pink 
#lower HSV : 112, 164, 19 
#Upper HSV : 176, 255, 255

#yellow
#lower HSV : 26, 192, 11 
#Upper HSV : 41, 255, 255

#green
#lower HSV : 41, 160, 51 
#Upper HSV : 61, 255, 255


def nothing(x):
    #
    pass

def camera_setup():
    print("Start Camera!")
    global cam
    cam = cv2.VideoCapture(0)  

def colorchoice(whatcolor = orange, graphic_enable = graphic_off):
    global lowerBound
    global upperBound
    for i in range(1):
        if whatcolor == pink:          # detect pink color
            lowerBound = np.array([112, 164, 19])
            upperBound = np.array([176, 255, 255])
            #print("Pink")
        elif whatcolor == yellow:       # detect yellow color
            lowerBound = np.array([24, 192, 44])        
            upperBound = np.array([35, 255, 255])
            #print("Yellow")
        elif whatcolor==green:       # detect green color
            lowerBound = np.array([36, 135, 128])
            upperBound = np.array([61, 255, 255])
            #print("Green")
        elif whatcolor==blue:       # detect blue color
            #lowerBound = np.array([62, 176, 125])       #daytime
            lowerBound = np.array([46, 209, 114])       #nighttime
            upperBound = np.array([109, 255, 255])
            #print("Blue")
        elif whatcolor==orange:       # detect orange color
            #lowerBound = np.array([12, 192, 174])       # daytime
            lowerBound = np.array([10, 191, 153])        # nighttime
            upperBound = np.array([24, 255, 255])
            #print("Orange")

        if(graphic_enable==graphic_on):
            cv2.namedWindow("Trackbars")                # Create Trackbars
            cv2.moveWindow("Trackbars", 10,20)          # Move it to (x,y)
            cv2.createTrackbar("Lower-H","Trackbars",lowerBound[0],180,nothing)
            cv2.createTrackbar("Lower-S","Trackbars",lowerBound[1],255,nothing)
            cv2.createTrackbar("Lower-V","Trackbars",lowerBound[2],255,nothing)
            cv2.createTrackbar("Upper-H","Trackbars",upperBound[0],180,nothing)
            cv2.createTrackbar("Upper-S","Trackbars",upperBound[1],255,nothing)
            cv2.createTrackbar("Upper-V","Trackbars",upperBound[2],255,nothing) 
    

def colorsearch(whatcolor = orange,graphic_enable = graphic_off):
        global lowerBound
        global upperBound
        global img,x,y,w,h,peri,shape
        ret, img = cam.read()
        img = cv2.resize(img, (screen_width, screen_hight))
        #cv2.imshow("Original",img)
        shape = "unidentified"

        if(graphic_enable==graphic_on):
            lower_H = cv2.getTrackbarPos("Lower-H","Trackbars")
            lower_V = cv2.getTrackbarPos("Lower-S","Trackbars")
            lower_S = cv2.getTrackbarPos("Lower-V","Trackbars")
            upper_H = cv2.getTrackbarPos("Upper-H","Trackbars")
            upper_S = cv2.getTrackbarPos("Upper-V","Trackbars")
            upper_V = cv2.getTrackbarPos("Upper-V","Trackbars")
            lowerBound = np.array([lower_H, lower_S, lower_V])
            upperBound = np.array([upper_H, upper_S, upper_V])

        kernelOpen = np.ones((5, 5))
        kernelClose = np.ones((20, 20))
        font = cv2.FONT_HERSHEY_SIMPLEX
        bottomLeftCornerOfText = (10,500)
        fontScale = 0.6
        fontColor = (0,0,255)
        lineType=2
   
   
        for i in range(1):
            ret, img = cam.read()
            img = cv2.resize(img, (screen_width, screen_hight))
              
            imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
            mask = cv2.inRange(imgHSV, lowerBound, upperBound)
            maskOpen = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernelOpen)
            maskClose = cv2.morphologyEx(maskOpen, cv2.MORPH_CLOSE, kernelClose)

            maskFinal = maskClose
            conts, h = cv2.findContours(maskFinal.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
            cv2.drawContours(img, conts, -1, (255, 0, 0), 3)
            for i in range(len(conts)):
                x, y, w, h = cv2.boundingRect(conts[i])
                if(w>15 ) :
                    for cnt in conts:
                        area=cv2.contourArea(conts[0])  
                        peri = cv2.arcLength(cnt, True)
                        approx = cv2.approxPolyDP(cnt, 0.04 * peri, True)
                        if(len(approx)==4):
                            shape='square'
                        elif (len(approx)==3):
                            shape='triangle'
                        elif(len(approx)==5):
                            shape='pentagon'
                        elif(len(approx)==6):
                            shape='hexagon'
                        elif(len(approx)>=7):
                            shape='polygon'
                else:
                    x=0
                    y=0
                    h=0
                    w=0
                    shape=0
                    peri=0
                    area=0
                    
            if(len(conts)==0):
                x=0
                y=0
                h=0
                w=0
                shape=0
                peri=0
                area=0
            return(x,y,h,w,shape,peri,whatcolor,area)   
                        
def showInMovedWindow(winname, img, x, y,graphic_enable= graphic_off):
    if(graphic_enable==graphic_on):
        cv2.namedWindow(winname)        # Create a named window
        cv2.moveWindow(winname, x, y)   # Move it to (x,y)
        cv2.imshow(winname,img)                       


def colorsequence(graphic_enable=graphic_off):
    font = cv2.FONT_HERSHEY_SIMPLEX
    bottomLeftCornerOfText = (10,500)
    fontScale = 0.6
    fontColor = (0,0,255)
    lineType=2
    arrayx1=np.array([])
    arrayy1=np.array([])
    arrayh1=np.array([])
    arrayw1=np.array([])
    arrayp1=np.array([])
    arrayx2=np.array([])
    arrayy2=np.array([])
    arrayh2=np.array([])
    arrayw2=np.array([])
    arrayp2=np.array([])
    success=0
        
    colorchoice(orange,graphic_enable)
    x1,y1,h1,w1,shape1,peri1,color1,area1=colorsearch(orange,graphic_enable)
    if(x1!=0 and y1!=0 and h1>=5 and w1>=5 and shape1=='square' and area1>100):
        
        colorchoice(blue,graphic_enable)
        x2,y2,h2,w2,shape2,peri2,color2,area2=colorsearch(blue,graphic_enable)
        if(x2!=0 and y2!=0 and h2>=5 and w2>=5 and shape1=='square'and area1>100):
            if(x1!=0) and (x2!=0):
                cv2.putText(img, 'Detect1', (x1, y1 - 10), font, fontScale, (155,255,0), 2)
                cv2.rectangle(img, (x1, y1), (x1 + w1, y1 + h1), (0, 0, 255), 2)
                #cv2.putText(img,"["+ str(round(x1,0)) +" ,  "+ str(round(y1,0)) +"]", (x1+w1+10, y1+10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1)
                #cv2.putText(img,"["+ str(round(h1,0)) +" ,  "+ str(round(w1,0)) +"]", (x1+w1+10, y1+30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,125,255), 1)

                cv2.putText(img, 'Detect2', (x2, y2 - 10), font, fontScale, (155,255,), 2)
                cv2.rectangle(img, (x2, y2), (x2 + w2, y2 + h2), (0, 0, 255), 2)
                #cv2.putText(img,"["+ str(round(x2,0))+ " ,  "+ str(round(y2,0)) +"]", (x2+w2+10, y2+10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1)
                #cv2.putText(img,"["+ str(round(h2,0))+ " ,  "+ str(round(w2,0)) +"]", (x2+w2+10, y2+30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,125,255), 1)
                #cv2.putText(img,"y2-y1["+ str(y2-y1) +"]"                 , (x2+w2+10, y2+h2+50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,125,255), 1)
                    
                if(math.isclose(y1+30,y2, abs_tol=60)):     # simple match as long as y2 is > y1 by +-30 pixels.                        
                    cv2.putText(img, 'Match within 60pixels', (x2, y2 + h2 + 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,255), 2)
                    success = 1
                else :
                    success = 0
                    x1=0
                    y1=0
                    h1=0
                    w1=0
                    area1=0
    cv2.line(img, (int(screen_width/2),10), (int(screen_width/2),screen_hight-10),(115,115,115), 1)
    cv2.line(img, (10,int(screen_hight/2)), (screen_width-10,int(screen_hight/2)),(115,115,115), 1)
    cv2.rectangle(img, (int(screen_width*0.4),int(screen_hight*0.4)), (int(screen_width*0.6),int(screen_hight*0.6)),(155,155,155), 1)
    showInMovedWindow("Detect",img,320,20,graphic_enable)
    x1=x1+w1/2
    y1=y1+h1/2
    return(success,x1,y1,h1,w1,area1) 

def camera_close():
    cam.release()
    cv2.destroyAllWindows()        


if(__name__=="__main__"):
    camera_setup()
    colorchoice(orange,graphic_on)
    fps = FPS().start()
    while(1):
        colorsearch(orange,graphic_on)
        showInMovedWindow("Orange",img,320,20,graphic_on)
        fps.update()
        k=cv2.waitKey(1) 
        if k == ord('x'):
            cv2.destroyWindow("Orange")
            break
    fps.stop()
    print("[INFO] elasped time: {:.2f}".format(fps.elapsed()))
    print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))        

    colorchoice(blue,graphic_on)
    fps = FPS().start()
    while(1):
        colorsearch(blue,graphic_on)
        showInMovedWindow("Blue",img,320,20,graphic_on)
        fps.update()
        k=cv2.waitKey(1) 
        if k == ord('x'):
            cv2.destroyWindow("Blue")
            break
    fps.stop()
    print("[INFO] elasped time: {:.2f}".format(fps.elapsed()))
    print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))        
    
    fps = FPS().start()
    for i in range(1000):
        colorsequence(graphic_on)
        fps.update()
        k=cv2.waitKey(1) 
        if k == ord('q'):
            break
        
    camera_close()
    fps.stop()
    print("[INFO] elasped time: {:.2f}".format(fps.elapsed()))
    print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))

I2C_LCD_driver.py

Python
Read RPI ifconfig IP address. Use during remote SSH connectivity with Visual Studio.
# -*- coding: utf-8 -*-
# Original code found at:
# https://gist.github.com/DenisFromHR/cc863375a6e19dce359d

"""
Compiled, mashed and generally mutilated 2014-2015 by Denis Pleic
Made available under GNU GENERAL PUBLIC LICENSE

# Modified Python I2C library for Raspberry Pi
# as found on http://www.recantha.co.uk/blog/?p=4849
# Joined existing 'i2c_lib.py' and 'lcddriver.py' into a single library
# added bits and pieces from various sources
# By DenisFromHR (Denis Pleic)
# 2015-02-10, ver 0.1

"""

# i2c bus (0 -- original Pi, 1 -- Rev 2 Pi)
I2CBUS = 1

# LCD Address
ADDRESS = 0x27

import smbus
from time import sleep

class i2c_device:
   def __init__(self, addr, port=I2CBUS):
      self.addr = addr
      self.bus = smbus.SMBus(port)

# Write a single command
   def write_cmd(self, cmd):
      self.bus.write_byte(self.addr, cmd)
      sleep(0.0001)

# Write a command and argument
   def write_cmd_arg(self, cmd, data):
      self.bus.write_byte_data(self.addr, cmd, data)
      sleep(0.0001)

# Write a block of data
   def write_block_data(self, cmd, data):
      self.bus.write_block_data(self.addr, cmd, data)
      sleep(0.0001)

# Read a single byte
   def read(self):
      return self.bus.read_byte(self.addr)

# Read
   def read_data(self, cmd):
      return self.bus.read_byte_data(self.addr, cmd)

# Read a block of data
   def read_block_data(self, cmd):
      return self.bus.read_block_data(self.addr, cmd)


# commands
LCD_CLEARDISPLAY = 0x01
LCD_RETURNHOME = 0x02
LCD_ENTRYMODESET = 0x04
LCD_DISPLAYCONTROL = 0x08
LCD_CURSORSHIFT = 0x10
LCD_FUNCTIONSET = 0x20
LCD_SETCGRAMADDR = 0x40
LCD_SETDDRAMADDR = 0x80

# flags for display entry mode
LCD_ENTRYRIGHT = 0x00
LCD_ENTRYLEFT = 0x02
LCD_ENTRYSHIFTINCREMENT = 0x01
LCD_ENTRYSHIFTDECREMENT = 0x00

# flags for display on/off control
LCD_DISPLAYON = 0x04
LCD_DISPLAYOFF = 0x00
LCD_CURSORON = 0x02
LCD_CURSOROFF = 0x00
LCD_BLINKON = 0x01
LCD_BLINKOFF = 0x00

# flags for display/cursor shift
LCD_DISPLAYMOVE = 0x08
LCD_CURSORMOVE = 0x00
LCD_MOVERIGHT = 0x04
LCD_MOVELEFT = 0x00

# flags for function set
LCD_8BITMODE = 0x10
LCD_4BITMODE = 0x00
LCD_2LINE = 0x08
LCD_1LINE = 0x00
LCD_5x10DOTS = 0x04
LCD_5x8DOTS = 0x00

# flags for backlight control
LCD_BACKLIGHT = 0x08
LCD_NOBACKLIGHT = 0x00

En = 0b00000100 # Enable bit
Rw = 0b00000010 # Read/Write bit
Rs = 0b00000001 # Register select bit

class lcd:
   #initializes objects and lcd
   def __init__(self):
      self.lcd_device = i2c_device(ADDRESS)

      self.lcd_write(0x03)
      self.lcd_write(0x03)
      self.lcd_write(0x03)
      self.lcd_write(0x02)

      self.lcd_write(LCD_FUNCTIONSET | LCD_2LINE | LCD_5x8DOTS | LCD_4BITMODE)
      self.lcd_write(LCD_DISPLAYCONTROL | LCD_DISPLAYON)
      self.lcd_write(LCD_CLEARDISPLAY)
      self.lcd_write(LCD_ENTRYMODESET | LCD_ENTRYLEFT)
      sleep(0.2)


   # clocks EN to latch command
   def lcd_strobe(self, data):
      self.lcd_device.write_cmd(data | En | LCD_BACKLIGHT)
      sleep(.0005)
      self.lcd_device.write_cmd(((data & ~En) | LCD_BACKLIGHT))
      sleep(.0001)

   def lcd_write_four_bits(self, data):
      self.lcd_device.write_cmd(data | LCD_BACKLIGHT)
      self.lcd_strobe(data)

   # write a command to lcd
   def lcd_write(self, cmd, mode=0):
      self.lcd_write_four_bits(mode | (cmd & 0xF0))
      self.lcd_write_four_bits(mode | ((cmd << 4) & 0xF0))

   # write a character to lcd (or character rom) 0x09: backlight | RS=DR<
   # works!
   def lcd_write_char(self, charvalue, mode=1):
      self.lcd_write_four_bits(mode | (charvalue & 0xF0))
      self.lcd_write_four_bits(mode | ((charvalue << 4) & 0xF0))
  
   # put string function with optional char positioning
   def lcd_display_string(self, string, line=1, pos=0):
    if line == 1:
      pos_new = pos
    elif line == 2:
      pos_new = 0x40 + pos
    elif line == 3:
      pos_new = 0x14 + pos
    elif line == 4:
      pos_new = 0x54 + pos

    self.lcd_write(0x80 + pos_new)

    for char in string:
      self.lcd_write(ord(char), Rs)

   # clear lcd and set to home
   def lcd_clear(self):
      self.lcd_write(LCD_CLEARDISPLAY)
      self.lcd_write(LCD_RETURNHOME)

   # define backlight on/off (lcd.backlight(1); off= lcd.backlight(0)
   def backlight(self, state): # for state, 1 = on, 0 = off
      if state == 1:
         self.lcd_device.write_cmd(LCD_BACKLIGHT)
      elif state == 0:
         self.lcd_device.write_cmd(LCD_NOBACKLIGHT)

   # add custom characters (0 - 7)
   def lcd_load_custom_chars(self, fontdata):
      self.lcd_write(0x40);
      for char in fontdata:
         for line in char:
            self.lcd_write_char(line)

DeliveryBox_WiFiAccessPoint6.ino

Arduino
Arduino NodeMCU code for SecuredDelivery Box with web apps
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <Servo.h>

Servo servoX,servoY;
/* Put your SSID & Password */
const char* ssid = "DeliverySecured";  // Enter SSID here
const char* password = "12345678";  //Enter Password here

/* Put IP Address details */
IPAddress local_ip(192,168,1,1);
IPAddress gateway(192,168,1,1);
IPAddress subnet(255,255,255,0);

ESP8266WebServer server(80);

uint8_t LED1pin = D0;
bool LED1status = LOW;

uint8_t LED2pin = D4;
bool LED2status = LOW;
long int count=0;
int open = 0;

//NodeMCU Label marking : GPIO #  //RFID
#define D0 16   // Built-in LED mid // magnetic lock
#define D1 5    //  Servo X Box1
#define D2 4    //  Servo Y Box2
#define D3 0  
#define D4 2    // LED wifi
#define D5 14   // Laser
#define D6 12   
#define D7 13   
#define D8 15   
#define D9 3    
#define D10 1 
#define D11 9   
#define D12 10

void client_status()
{
 
unsigned char number_client;
struct station_info *stat_info;
uint8_t mac_add[6];
struct ip_addr *IPaddress;
IPAddress address;
 
//WiFi.softAPgetStationNum();
WiFi.softAPmacAddress(mac_add);

Serial.print(" Total connected_client are = ");
Serial.println(WiFi.softAPgetStationNum());
Serial.print("Mac Address of the Device");
Serial.println(WiFi.softAPmacAddress());
}

void setup() {
  Serial.begin(115200);
  pinMode(LED1pin, OUTPUT);
  pinMode(LED2pin, OUTPUT);
  
  WiFi.softAP(ssid, password);
  WiFi.softAPConfig(local_ip, gateway, subnet);
  delay(100);
  
  server.on("/", handle_OnConnect);
  server.on("/led1on", handle_led1on);
  server.on("/led1off", handle_led1off);
  server.on("/led2on", handle_led2on);
  server.on("/led2off", handle_led2off);
  server.onNotFound(handle_NotFound);
  
  server.begin();
  Serial.println("HTTP server started");
  servoX.attach(D1);
  servoX.write(30);
  Serial.println("Box1 Open");
  delay(1000);
  servoX.write(130);
  Serial.println("Box1 Close");
  delay(2000);
  
  servoY.attach(D2);
  servoY.write(10);
  Serial.println("Box2 Open");
  delay(1000);
  servoY.write(160);
  Serial.println("Box2 Close");
  delay(1000);

}

void loop() {
 // if(open==1) {
  //  count++;
   // if(count>500000){
   //   Serial.println("Automatic Close After certain duration");
     // servoX.write(130);   //close the box
   //   count=0;
   //   open =0;
   //  }
 // }

  server.handleClient();
  if(LED1status)
  {
    digitalWrite(LED1pin, HIGH);
  }
  else
  {
    digitalWrite(LED1pin, LOW);
  }
  
  if(LED2status)
  {digitalWrite(LED2pin, HIGH);}
  else
  {digitalWrite(LED2pin, LOW);}
}



void handle_OnConnect() {
  LED1status = LOW;
  LED2status = LOW;
  Serial.println("Connected");
  Serial.println("GPIO7 Status: OFF | GPIO6 Status: OFF");
  server.send(200, "text/html", SendHTML(LED1status,LED2status)); 
}

void handle_led1on() {
  LED1status = HIGH;
  Serial.println("Box1 Status: Open");
  server.send(200, "text/html", SendHTML(true,LED2status)); 
  servoX.write(30);
  open=1;
  count=0;
  
}

void handle_led1off() {
  LED1status = LOW;
  Serial.println("Box1 Status: Close");
  server.send(200, "text/html", SendHTML(false,LED2status)); 
  servoX.write(130);
  open=0;

}

void handle_led2on() {
  LED2status = HIGH;
  Serial.println("Box2 Status: Open");
  server.send(200, "text/html", SendHTML(LED1status,true)); 
  servoY.write(10);
}

void handle_led2off() {
  LED2status = LOW;
  Serial.println("Box2 Status: Close");
  server.send(200, "text/html", SendHTML(LED1status,false)); 
  servoY.write(160);

}

void handle_NotFound(){
  server.send(404, "text/plain", "Not found");
}

String SendHTML(uint8_t led1stat,uint8_t led2stat){
  String ptr = "<!DOCTYPE html> <html>\n";
  ptr +="<head><meta name=\"viewport\" content=\"width=device-width, initial-scale=1.0, user-scalable=no\">\n";
  ptr +="<title>LED Control</title>\n";
  ptr +="<style>html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}\n";
  ptr +="body{margin-top: 50px;} h1 {color: #444444;margin: 50px auto 30px;} h3 {color: #444444;margin-bottom: 50px;}\n";
  ptr +=".button {display: block;width: 80px;background-color: #1abc9c;border: none;color: white;padding: 13px 30px;text-decoration: none;font-size: 25px;margin: 0px auto 35px;cursor: pointer;border-radius: 4px;}\n";
  ptr +=".button-on {background-color: #00d000;}\n";
  ptr +=".button-on:active {background-color: #00d000;}\n";
  ptr +=".button-off {background-color: #fd3e50;}\n";
  ptr +=".button-off:active {background-color: #fd3e50;}\n";
  ptr +="p {font-size: 14px;color: #888;margin-bottom: 10px;}\n";
  ptr +="</style>\n";
  ptr +="</head>\n";
  ptr +="<body>\n";
  ptr +="<h1>Delivery Secured Box</h1>\n";
  ptr +="<h3>Please select to Open or Close the Delivery Box</h3>\n";
  
   if(led1stat)
  {ptr +="<p>Box1 Status: Open</p><a class=\"button button-off\" href=\"/led1off\">CLOSE</a>\n";}
  else
  {ptr +="<p>Box1 Status: Close</p><a class=\"button button-on\" href=\"/led1on\">OPEN</a>\n";}

  if(led2stat)
  {ptr +="<p>Box2 Status: Open</p><a class=\"button button-off\" href=\"/led2off\">CLOSE</a>\n";}
  else
  {ptr +="<p>Box2 Status: Close</p><a class=\"button button-on\" href=\"/led2on\">OPEN</a>\n";}

  ptr +="</body>\n";
  ptr +="</html>\n";
  return ptr;
}

All codes are available in Github.

Click and Download

Credits

Yong Voon Siew

Yong Voon Siew

1 project • 3 followers
Thanks to https://www.bensound.com/ and Adrian Rosebrock.

Comments

Related channels and tags
  • Cytron Technologies
  • Espressif
  • Microsoft
  • NodeMCU
  • OpenCV
  • Raspberry Pi