Sign In

  • Username:
  • Password:

Upload File

  •  

Automatic Aiming and Tracking BB Gun with OpenCV and PID

Having an automatic aiming artillery is every boy’s dream. This semester’s Automatic Control and Visual Objects Tracking course have taught me some principles of tracking and pan/tilt control. So it’s time to do it!

0x01 Prepare

Software

  • Arduino IDE
  • OpenCV 3
  • Golang

Libraries

Hardware

  • Arduino Nano and Expansion Shield.
  • USB Camera(UVC supported)
  • SG90 * 2(cheap servo is enough)
  • Pan-Tilt
  • BB Gun

0x02 Tracking

In this project, I mainly focused on the control system, so I directly called the OpenCV’s tracking API. If there is time, I’d like to try DNN.

Tracking Example in GoCV

PS: KCF is better.

0x03 Pan/Tilt Control

In the first version without PID, the control of pan/tilt is only according to the relationship between the position of the target and the screen center.

//Find out if the X component of the target is to the left of the middle of the screen.
if(valx < (screenmaxx/2 - 1)){
  if( posx >= 5 ) posx += distancex; //Update the pan position variable to move the servo to the left.
}
// X right
else if(valx > screenmaxx/2 + 1){
  if(posx <= servomaxx-incx) posx -= distancex;
}
// Y bellow
if(valy > (screenmaxy/2 + 1)){
  if(posy >= 5)posy += distancey;
}
// Y above
else if(valy < (screenmaxy/2 - 1)){
  if(posy <= servomaxy)posy -= distancey;
}

Simple and straightforward, right? But there is a fatal flaw in this system - ξ=0, which will cause equal amplitude oscillation. The camera can track but can never aim.

The reason may be that the relative position judging speed is much faster than the servos.(Cheap analog servos may also be a cause, but it has little impact here.)

But if we use PID?

Perfect!

Dynamic characteristic curve can say more :)

So what is PID?

A proportional–integral–derivative controller (PID controller or three term controller) is a control loop feedback mechanism widely used in industrial control systems and a variety of other applications requiring continuously modulated control. A PID controller continuously calculates an error value e(t) as the difference between a desired setpoint (SP) and a measured process variable (PV) and applies a correction based on proportional, integral, and derivative terms (denoted P, I, and D respectively) which give the controller its name.(from wiki)

And the transfer function in the Laplace domain of the PID controller is L(s) = Kp + Ki/s + Kd*s

The PID controller is a feedback loop component that is common in industrial control applications. The controller compares the collected data with a reference value and then uses this difference to calculate a new input value. The purpose of this new input value is to allow the system data to be reached or maintained at the reference value. The PID controller can adjust the input value according to the historical data and the occurrence rate of the difference, making the system more accurate and stable.

Here’s the impact of each parameter.(from my teacher)

And it’s easy to use PID in arduino.

#include <PID_v1.h>

double Setpoint, Input_X, Output_X;
const double kp = 0.062, ki = 0.0005, kd = 0;

PID myPID_X(&Input_X, &Output_X, &Setpoint, kp, ki, kd, DIRECT);

void setup()
{
    Setpoint = 0;
    myPID_X.SetOutputLimits(-255.0,255);
    myPID_X.SetMode(AUTOMATIC);
}

void loop ()
{
    Input_X = centerX - screenmaxX/2;
    myPID_X.Compute();
    posx += 0.15*Output_X;
}

0x04 Communication

As we only need to transfer the position(0~640), so 4 byte is enough.

while(Serial.available() >= 4)  // wait for 4 bytes. 
{
    // get X axis 2-byte integer from serial
    MSB = Serial.read();
    delay(2);
    LSB = Serial.read();
    delay(2);
    MSBLSB=word(MSB, LSB);
    Input_X = MSBLSB-screenmaxx/2;

    // get Y axis 2-byte integer from serial
    MSB = Serial.read();
    delay(2);
    LSB = Serial.read();
    delay(2);
    MSBLSB=word(MSB, LSB);
    Input_Y = MSBLSB-screenmaxy/2;
}

0x05 All Code

tracking.go

package main

import (
    "encoding/binary"
    "fmt"
    "image/color"
    "log"
    "time"

    "github.com/tarm/serial"
    "gocv.io/x/gocv"
    "gocv.io/x/gocv/contrib"
)

var (
    centerX  uint16
    centerY  uint16
    deviceID = 2 // send X axis LSB
    baudrate = 9600
    port     = "/dev/ttyUSB0"
)

func main() {
    // open webcam
    webcam, err := gocv.OpenVideoCapture(deviceID)
    if err != nil {
        fmt.Printf("error opening video capture device: %v\n", deviceID)
        return
    }
    defer webcam.Close()

    c := &serial.Config{Name: port, Baud: baudrate}
    s, err := serial.OpenPort(c)
    if err != nil {
        log.Fatal(err)
    }

    // open display window
    window := gocv.NewWindow("Tracking")
    defer window.Close()
    window.ResizeWindow(640, 480)
    // create a tracker instance
    // (one of MIL, KCF, TLD, MedianFlow, Boosting, MOSSE or CSRT)
    tracker := contrib.NewTrackerKCF()
    defer tracker.Close()

    // prepare image matrix
    img := gocv.NewMat()
    defer img.Close()

    // read an initial image
    if ok := webcam.Read(&img); !ok {
        fmt.Printf("cannot read device %v\n", deviceID)
        return
    }

    // let the user mark a ROI to track
    rect := gocv.SelectROI("Tracking", img)
    if rect.Max.X == 0 {
        fmt.Printf("user cancelled roi selection\n")
        return
    }

    // initialize the tracker with the image & the selected roi
    init := tracker.Init(img, rect)
    if !init {
        fmt.Printf("Could not initialize the Tracker")
        return
    }

    // color for the rect to draw
    blue := color.RGBA{0, 0, 255, 0}
    fmt.Printf("start reading camera device: %v\n", deviceID)

    valX := make([]byte, 2)
    valY := make([]byte, 2)

    for {
        if ok := webcam.Read(&img); !ok {
            fmt.Printf("cannot read device %v\n", deviceID)
            return
        }
        if img.Empty() {
            continue
        }

        // update the roi
        rect, _ := tracker.Update(img)

        // draw it.
        gocv.Rectangle(&img, rect, blue, 3)

        // show the image in the window, and wait 10 millisecond
        window.IMShow(img)
        if window.WaitKey(10) >= 0 {
            break
        }

        // rect center
        centerX = uint16(rect.Min.Add(rect.Max).X / 2)
        centerY = uint16(rect.Min.Add(rect.Max).Y / 2)

        if centerX == 0 || centerY == 0 {
            continue
        }

        binary.LittleEndian.PutUint16(valX, centerX)
        binary.LittleEndian.PutUint16(valY, centerY)

        // send X axis MSB
        _, err := s.Write([]byte{valX[1]})
        time.Sleep(2 * time.Millisecond)

        // send X axis LSB
        _, err = s.Write([]byte{valX[0]})
        time.Sleep(2 * time.Millisecond)

        // send Y axis MSB
        _, err = s.Write([]byte{valY[1]})
        time.Sleep(2 * time.Millisecond)

        // send Y axis LSB
        _, err = s.Write([]byte{valY[0]})
        time.Sleep(2 * time.Millisecond)

        if err != nil {
            log.Fatal(err)
        }

        time.Sleep(10 * time.Millisecond)
    }
}

pt.ino

#include <Servo.h>
#include <PID_v1.h>

#define  screenmaxx   640   // max screen horizontal (x)resolution
#define  screenmaxy   480    // max screen vertical (y) resolution
#define  servocenterx 75  // center po#define  of x servo
#define  servocentery 60  // center po#define  of y servo
#define  servopinx    9   // digital pin for servo x
#define  servopiny    10  // digital servo for pin y
#define  baudrate     9600  // com port speed. Must match your C++ setting

double posx,posy;

Servo servox;
Servo servoy;

double Setpoint, Input_X, Output_X, Input_Y, Output_Y;
const double kp = 0.062, ki = 0.0005, kd = 0;

PID myPID_X(&Input_X, &Output_X, &Setpoint, kp, ki, kd, DIRECT);
PID myPID_Y(&Input_Y, &Output_Y, &Setpoint, kp, ki, kd, DIRECT);

short MSB = 0;  // to build  2 byte integer from serial in byte
short LSB = 0;  // to build  2 byte integer from serial in byte
int   MSBLSB = 0;  //to build  2 byte integer from serial in byte

void setup() {

  Serial.begin(baudrate);        // connect to the serial port

  pinMode(servopinx,OUTPUT);    // declare the LED's pin as output
  pinMode(servopiny,OUTPUT);    // declare the LED's pin as output

  servoy.attach(servopiny); 
  servox.attach(servopinx); 

  // center servos

  servox.write(servocenterx); 
  delay(200);
  servoy.write(servocentery); 
  delay(200);

  Setpoint = 0; //desired speed value to keep RPMs

  myPID_X.SetOutputLimits(-255.0,255);  // range for servo.writeMicroseconds()
  myPID_Y.SetOutputLimits(-255.0,255);  // range for servo.writeMicroseconds()

  myPID_X.SetMode(AUTOMATIC); //turn PID on
  myPID_Y.SetMode(AUTOMATIC); //turn PID on
}

void loop () {
  while(Serial.available() >= 4)  // wait for 4 bytes. 
  {
    // get X axis 2-byte integer from serial
    MSB = Serial.read();
    delay(2);
    LSB = Serial.read();
    delay(2);
    MSBLSB=word(MSB, LSB);
    Input_X = MSBLSB-screenmaxx/2;

    // get Y axis 2-byte integer from serial
    MSB = Serial.read();
    delay(2);
    LSB = Serial.read();
    delay(2);
    MSBLSB=word(MSB, LSB);
    Input_Y = MSBLSB-screenmaxy/2;

    // read current servos positions
    posx = servox.read(); 
    posy = servoy.read();

    myPID_X.Compute();
    myPID_Y.Compute();

    posx += 0.15*Output_X;
    posy -= 0.15*Output_Y;

    servox.write(posx);
    servoy.write(posy);
  }   
}