Squabo - The Machine Vision Robot

# colorTrackerChassis - By: Sahil Rastogi - Sat Aug 29 2020

# b[6] is cy which will be used for left and right movement

import sensor
import image
import lcd
import time
from Maix import GPIO
from fpioa_manager import fm
from machine import Timer,PWM

# enable pins of motor driver

m1EnPin = 21 # pin 2 of maixduino
m2EnPin = 15 # pin 7 of maixduino
m3EnPin = 14 # pin 8 of maixduino
m4EnPin = 3  # pin 13 of maixduino

fm.register(m1EnPin, fm.fpioa.GPIOHS0)
fm.register(m2EnPin, fm.fpioa.GPIOHS1)
fm.register(m3EnPin, fm.fpioa.GPIOHS2)
fm.register(m4EnPin, fm.fpioa.GPIOHS3)

#control pins of motor driver

m1Pin1 = 22 # pin 3 of maixduino
m1Pin2 = 23 # pin 4 of maixduino
m2Pin1 = 24 # pin 5 of maixduino
m2Pin2 = 32 # pin 6 of maixduino
m3Pin1 = 13 # pin 9 of maixduino
m3Pin2 = 12 # pin 10 of maixduino
m4Pin1 = 11 # pin 11 of maixduino
m4Pin2 = 10 # pin 12 of maixduino

fm.register(m1Pin1, fm.fpioa.GPIO0)
fm.register(m1Pin2, fm.fpioa.GPIO1)
fm.register(m2Pin1, fm.fpioa.GPIO2)
fm.register(m2Pin2, fm.fpioa.GPIO3)
fm.register(m3Pin1, fm.fpioa.GPIO4)
fm.register(m3Pin2, fm.fpioa.GPIO5)
fm.register(m4Pin1, fm.fpioa.GPIO6)
fm.register(m4Pin2, fm.fpioa.GPIO7)

m1p1 = GPIO(GPIO.GPIO0,GPIO.OUT)
m1p2 = GPIO(GPIO.GPIO1,GPIO.OUT)
m2p1 = GPIO(GPIO.GPIO2,GPIO.OUT)
m2p2 = GPIO(GPIO.GPIO3,GPIO.OUT)
m3p1 = GPIO(GPIO.GPIO4,GPIO.OUT)
m3p2 = GPIO(GPIO.GPIO5,GPIO.OUT)
m4p1 = GPIO(GPIO.GPIO6,GPIO.OUT)
m4p2 = GPIO(GPIO.GPIO7,GPIO.OUT)

timer1 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode = Timer.MODE_PWM)
timer2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode = Timer.MODE_PWM)
timer3 = Timer(Timer.TIMER0, Timer.CHANNEL2, mode = Timer.MODE_PWM)
timer4 = Timer(Timer.TIMER0, Timer.CHANNEL3, mode = Timer.MODE_PWM)

m1En = PWM(timer1, 500000, 0, m1EnPin)
m2En = PWM(timer2, 500000, 0, m2EnPin)
m3En = PWM(timer3, 500000, 0, m3EnPin)
m4En = PWM(timer4, 500000, 0, m4EnPin)

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.run(1)

def moveCW(mName):
    if(mName == 'm1'):
        m1p1.value(1)
        m1p2.value(0)
    elif(mName == 'm2'):
        m2p1.value(1)
        m2p2.value(0)
    elif(mName == 'm3'):
        m3p1.value(1)
        m3p2.value(0)
    elif(mName == 'm4'):
        m4p1.value(1)
        m4p2.value(0)

def moveCCW(mName):
    if(mName == 'm1'):
        m1p1.value(0)
        m1p2.value(1)
    elif(mName == 'm2'):
        m2p1.value(0)
        m2p2.value(1)
    elif(mName == 'm3'):
        m3p1.value(0)
        m3p2.value(1)
    elif(mName == 'm4'):
        m4p1.value(0)
        m4p2.value(1)

def hold(mName):
    if(mName == 'm1'):
        m1p1.value(0)
        m1p2.value(0)
    elif(mName == 'm2'):
        m2p1.value(0)
        m2p2.value(0)
    elif(mName == 'm3'):
        m3p1.value(0)
        m3p2.value(0)
    elif(mName == 'm4'):
        m4p1.value(0)
        m4p2.value(0)

def moveForward():
    moveCW('m1')
    moveCW('m2')
    moveCCW('m3')
    moveCCW('m4')

def moveBackward():
    moveCCW('m1')
    moveCCW('m2')
    moveCW('m3')
    moveCW('m4')

def turnLeft():
    moveCW('m1')
    moveCW('m2')
    moveCW('m3')
    moveCW('m4')

def turnRight():
    moveCCW('m1')
    moveCCW('m2')
    moveCCW('m3')
    moveCCW('m4')

def stop():
    hold('m1')
    hold('m2')
    hold('m3')
    hold('m4')

def setSpeed(dutyCycle):
    m1En.duty(dutyCycle)
    m2En.duty(dutyCycle)
    m3En.duty(dutyCycle)
    m4En.duty(dutyCycle)

colorThreshold = (21, 58, 30, 127, -128, 127) #for dobby's red spiked ball

f = 350
width = 2.75

'''calculated focal length came out to be 350 pixels.
   f = (p*d)/w
   f is calculated focal length of camera
   p is perceived width measured from camera i.e. b[2]
   d is actual distance which was 7" at time of measurement
   w is actual width of object which was 2.75" for dobby's red spiked ball'''

'''distance formula is
   d' = (f*w)/p
   where d' is perceived/calculated distance of object from camera'''

while True:
    img = sensor.snapshot()
    blobs = img.find_blobs([colorThreshold], pixels_threshold = 100, area_threshold = 150, merge = True)
    if blobs:
        for b in blobs:
            img.draw_rectangle(b[0:4],(255,0,0),3)
            p = b[2]
            d = (f*width)/p
            if(d >= 11):
                setSpeed(25)
                moveForward()
                print('Move forward')
            elif(d <= 7):
                setSpeed(25)
                moveBackward()
                print('Move backward')
            if(b[6] >= 20 and b[6] < 70):
                setSpeed(25)
                turnRight()
                print('Turn right.')
            elif(b[6] > 170 and b[6] <= 220):
                setSpeed(25)
                turnLeft()
                print('Turn left.')
            if(b[6] >= 70 and b[6] <= 170 and d > 7 and d < 11):
                setSpeed(0)
                stop()
                print('Stop')
    else:
        setSpeed(0)
        stop()
        print('No object found.')
    lcd.display(img)