fargetracking_med_servo_hjul

# This example shows off single color RGB565 tracking using the OpenMV Cam.

import sensor, image, time
from pyb import Servo                       # Importerer servokontroll
from pyb import UART                        # Importerer nødvendig bibliotek for å kommunisere serielt
uart = UART(1,9600, timeout_char = 1000)    # Definerer kommunikasjon med Arduino

threshold_index = 1 # 0 for red, 1 for green, 2 for blue

# Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max)
# The below thresholds track in general red/green/blue things. You may wish to tune them...
thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds
              (27, 100, -74, -25, 13, 62), # generic_green_thresholds DISSE VERDIENE ER FORANDRET
              (0, 30, 0, 64, -128, 0)] # generic_blue_thresholds

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()

x_pos = 1500    # Definerer verdien til startposisjon - Pan
y_pos = 1500    # Definerer verdien til startposisjon - Tilt

x_min = 640     # Laveste pulsbreddesignal ved panorering (-90)
x_max = 2500    # Høyeste pulsbreddesignal ved panorering (90)
y_max = 2500    # Laveste pulsbreddesignal ved tilt (-90)
y_min = 640     # Høyeste pulsbreddesignal ved tilt (90)

x_gain = -.75   # Justerer følsomheten til panoreringen, negativ verdi for å følge objektet. (Kanskje justere ned?)
y_gain = -.75   # Justerer følsomheten til tiltingen, negativ verdi for å følge objektet.    (Kanskje justere ned?)

xServo = Servo(1)           # Definerer Servo 1 (pan, pin P7 på OpenMV M7)
yServo = Servo(2)           # Definerer Servo 2 (tilt, pin P8 på OpenMV M7)

xServo.pulse_width(x_pos)   # Setter Servo 1 til startposisjon (fremover)
yServo.pulse_width(y_pos)   # Setter Servo 2 til startposisjon (rett opp)

# Only blobs that with more pixels than "pixel_threshold" and more area than "area_threshold" are
# returned by "find_blobs" below. Change "pixels_threshold" and "area_threshold" if you change the
# camera resolution. "merge=True" merges all overlapping blobs in the image.

while(True):
    clock.tick()
    img = sensor.snapshot()
    for blob in img.find_blobs([thresholds[threshold_index]], pixels_threshold=200, area_threshold=200, merge=True):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())

        x = blob.cx()       # Finner x-posisjonen til sentrum av objekt
        y = blob.cy()       # Finner y-posisjonen til sentrum av objekt
        f = blob.rect()     # Inneholder verdiene til rektangelet rundt objektetet (x, y, bredde, høyde)

        x_error = x - (img.width()/2)   # Regner ut avstand fra sentrum
        y_error = y - (img.height()/2)  # Regner ut avstand fra sentrum

        x_pos += x_error * x_gain       # Justerer og angir ny servoposisjon (x)
        y_pos += y_error * y_gain       # Justerer og angir ny servoposisjon (y)

        if (x_pos > x_max):             # Hvis man når ytre venstrekant
            x_pos = x_max               # Sett x_pos til maksimal PWM (90 grader)
            uart.write('venstre')       # Send kommando til Arduino som svinger til venstre (FORANDRE 'venstre' TIL NOE)
        if (x_pos < x_min):             # Hvis man når ytre høyrekant
            x_pos = x_min               # Sett x_pos til laveste PWM (-90 grader)
            uart.write('hoyre')         # Send kommando til Arudino som svinger til høyre (FORANDRE 'hoyre' TIL NOE)

        if (y_pos > y_max):             # Hvis man når maksimal tilt (Opp?)
            y_pos = y_max               # Sett y_pos til høyeste PWM (90)
        if (y_pos < y_min):             # Hvis man når laveste tilt (Ned?)
            y_pos = y_min               # sett y_pos til laveste PWM (-90)

        if (f[2] < 60):                 # Hvis bredden på objektet (pixler) blir liten (Verdien må testes og justeres)
            uart.write('fremover')      # Sender kommando til Arduino som trigger fremover (FORANDRE 'fremover' TIL NOE)
        if (f[2] > 80:                  # Hvis bredden på objektet (pixler) blir stor (Verdien må testes og justeres)
            uart.write('bakover')       # Sender kommando til Arduino som trigger revers (FORANDRE 'bakover' TIL NOE)

        xServo.pulse_width(int(x_pos))  # Beveger servoen (pan) mot objekt
        yServo.pulse_width(int(y_pos))  # Beveger servoen (tilt) mot objekt

    # Draw FPS
    img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))