Paintball gun code

The following is the code we used for our project. Arduino code is listed first (X Servo then Y Servo) followed by OpenCV.

To download our code, click here.

 

// Author: Mark Carroll and Bobby Cole

// Turns the X Servo to the desired degree

 

int state = 0;

 

 

#include <Servo.h>

int X_MIN;                                                                                                                                                          // minimum angle servos can turn based on camera viewing angle

int X_MAX;                                                                                                                                                         // maximum angle

Servo myServo1;

const double secondsPerDegree = 5.5556;                                           // since the method we use to write to servos has values between 1000 and 2000, had to convert this

                                                                                                                                                                                                // range into a degree measure (1000/180)

const double degreesPerSecond = 0.18;                                                // (180/1000)

int currentAngleServo1;

int lsbX;                                                                                                                                                                // least significant 8 bits of x coordinate from serial

int msbX;                                                                                                                                                             // most significant 8 bits

int xCoor;                                                                                                                                                            // combined the lsbX and msbX                               

 

int start=2;

int ending=9;

 

int x=1;

int xyp=0;

int buttonState = 0;

 

// sets pins to input or output, sets a baud rate for serial communication, and attaches X Servo to pin 9

void setup() {

  pinMode(2, INPUT);

  pinMode(3, INPUT);

  pinMode(4, INPUT);

  pinMode(5,INPUT);

  pinMode(6, INPUT);

  pinMode(7, OUTPUT);

  pinMode(8, OUTPUT);

  pinMode(11, OUTPUT);

  pinMode(12, OUTPUT);

  pinMode(13, OUTPUT);

  Serial.begin(9600);

  myServo1.attach(9);

}

 

void loop() {

  if(state == 0){                                                                                                                                                                                                  //resetting X Servo back to 0

    writeDegrees("myServo1", 0);

    state = 1;

    delay(250);

    // up 2

    //down 3

    //left 4

    //right 5

  }else if(state == 1){                                                                                                                                                                                       //setting X_MIN and Y_MIN

     if(digitalRead(4) == HIGH){                                                                                                                                     // pressing button attached to 4 moves gun by 1 degree to the right

        writeDegrees("myServo1", currentAngleServo1 + 1);

        delay(25);

     }else if(digitalRead(5) == HIGH){                                                                                                                          // pressing button attached to 5 moves gun by 1 degree to the left

        writeDegrees("myServo1", currentAngleServo1 -1);

        delay(25);

     }else if(digitalRead(6) == HIGH){                                                                                                                          // sets either the X_MIN or X_MAX

        if(buttonState == 0){                                                                                                                                                             

          X_MIN = currentAngleServo1;

          buttonState = 1;

          delay(500);

        }else{

          X_MAX = currentAngleServo1;

          state = 2;

        }

     }

  }else if(state == 2){

       if(Serial.available() > 0){

         if(Serial.read() == 255){                                                                                                                                                        // signifies to start sending data to arduinos (255 is just what we decided to send)

            state = 3;

         }

       }

  }else if(state == 3){

    if(Serial.available() > 0){                                                                                                                                                                            // if serial is available, read in the 8 least significant bits

        lsbX = Serial.read();

        state = 4;

     }

  }else if(state == 4){

     if(Serial.available() > 0){                                                                                                                                                           // read in 8 most significant bits

        msbX = Serial.read();

        msbX<<=8;

        xCoor = lsbX + msbX;                                                                                                                                                                              // combines lsb and msb

        if(xCoor < 320 && xCoor > 0){                                                                                                                                             // checks bounds

          int tempServo1 = (int)((X_MAX-X_MIN)*((double)xCoor/(double)320));                                                                                    // because of our gears, had to do some magic calculations

          tempServo1 = X_MAX - tempServo1;

          writeDegrees("myServo1", tempServo1);

       }

        state = 2;

     } 

  }

}

 

void writeDegrees(String servoName, int degree){

  // 1000 is 180 degrees; 2000 is 0 degrees; 1500 is 90 degrees

  // every microsecond is .18 degrees

  if(degree == 0){

      //myServo1.writeMicroseconds(1000);                                                                           // can uncomment this to make sentry go back to same location everytime it detects no motion

  }

  if(currentAngleServo1 < degree){                                                                                                          // rotates servo counterclockwise

    if(degree >= 0 && degree <=180){                                                                                       // checks bounds

                  // we used a loop here to make the servo move more smoothly

      for(int i = (1000+(currentAngleServo1/degreesPerSecond)); i <= (1000+(degree/degreesPerSecond)); i++){

          myServo1.writeMicroseconds(i);

          delay(1);

      }

    }

    updateAngle(servoName,degree);                                                                                                     // updates servo angle

  }else if(currentAngleServo1 > degree){                                                                               // rotates servo clockwise

    if(degree >= 0 && degree <=180){

      for(int i = (1000+(currentAngleServo1/degreesPerSecond)); i >= (1000+(degree/degreesPerSecond)); i--){

          myServo1.writeMicroseconds(i);

          delay(1);

      }

    }

    updateAngle(servoName,degree);

 

  }

}

 

// keeps track of the last degree measure written to the X Servo

void updateAngle(String servoName, int degree){

currentAngleServo1 = degree;

}

 

 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

 

 

 

// Author: Mark Carroll and Bobby Cole

// Turns the Y Servo and trigger servo

 

int state = 0;

 

 

#include <Servo.h>

int timeSpentShooting;

int Y_MIN;                                                                                                                                                                                                          // MIN and MAX for Y Servo

int Y_MAX;

Servo yServo;

Servo shootServo;

const double secondsPerDegree = 5.5556;                                                                                           // since the method we use to write to servos has values between 1000 and 2000, had to convert this

                                                                                                                                                                                                                                                // range into a degree measure (1000/180)

const double degreesPerSecond = 0.18;                                                                                                // (180/1000)

int currentAngleServo2;

int lsbY;                                                                                                                                                                                                                // least significant 8 bits of y coordinate from serial

int msbY;                                                                                                                                                                                                             // most significant 8 bits

int yCoor;                                                                                                                                                                                                            // combined the lsbY and msbY

int toShoot=0;

int shootNeutral = 86;                                                                                                                                                    // steady state for shoot servo

 

int start=2;

int ending=9;

 

int x=1;

int xyp=0;

int buttonState = 0;

 

// sets up pins for input, output, serial communication, and attaches servos

void setup() {                                                                                                                                                                                    

  pinMode(2, INPUT);

  pinMode(3, INPUT);

  pinMode(4, INPUT);

  pinMode(5,INPUT);

  pinMode(6, INPUT);

  pinMode(7, OUTPUT);

  pinMode(8, OUTPUT);

  pinMode(11, OUTPUT);

  pinMode(12, OUTPUT);

  pinMode(13, OUTPUT);

  Serial.begin(9600);

  yServo.attach(10);

  shootServo.attach(9);

}

 

void loop() {

  if(state == 0){                                                                                                                                                                                                                  //resetting servos back to 0

    writeDegrees("yServo", 90);

    shootServo.write(shootNeutral);                                                                                                                                                         // stops shooting servo

    state = 1;

    delay(250);

  }else if(state == 1){                                                                                                                                                                                                       //setting X_MIN and Y_MIN

     if(digitalRead(2) == HIGH){

        writeDegrees("yServo", currentAngleServo2 - 1);                                                                     // move y servo down by 1 degree

        shootServo.write(shootNeutral);

        delay(25);

     }else if(digitalRead(3) == HIGH){

        writeDegrees("yServo", currentAngleServo2 + 1);                                                                    // move y servo up by 1 degree

        shootServo.write(shootNeutral);

        delay(25);

     }else if(digitalRead(6) == HIGH){                                                                                                                                          // pressing this button sets Y_MAX and MIN

        shootServo.write(shootNeutral);

        if(buttonState == 0){

          Y_MIN = currentAngleServo2;

          buttonState = 1;

          delay(500);

        }else{

          Y_MAX = currentAngleServo2;

          state = 2;

        }

     }

  }else if(state == 2){

       if(Serial.available() > 0){

         if(Serial.read() == 255){                                                                                                                                        // signifies to start sending data to arduinos (255 is just what we decided to send)

            state = 3;

         }

       }

  }else if(state == 3){

    if(Serial.available() > 0){                                                                                                                                                                                            // if serial is available, read in the 8 least significant bits

        lsbY = Serial.read();

        state = 4;

     }

  }else if(state == 4){

     if(Serial.available() > 0){                                                                                                                                                                           // read in 8 most significant bits

        msbY = Serial.read();

        msbY<<=8;

        yCoor = lsbY + msbY;                                                                                                                                                                                              // combines lsb and msb bits

        if(yCoor < 240 && yCoor > 0){                                                                                                                                                             // checks bounds

          int tempServo1 = (int)((Y_MAX-Y_MIN)*((double)yCoor/(double)240));                                                    // because of our gears, had to do some magic calculations

          tempServo1 = Y_MIN + tempServo1;

          writeDegrees("yServo", tempServo1);                                                                                                       

       }

        //state = 5;

        state = 5;

     } 

  }else if(state == 5){

    if(Serial.available() > 0){

       toShoot = Serial.read();

       if(toShoot > 0){                                                                                                                                                                                                          // turns shooting servo

         timeSpentShooting = millis();

         shootServo.write(180);

         //delay(10);

       }else{

         if((millis()-timeSpentShooting) > 4000){                                                                                        // stops shooting after 4 seconds

           shootServo.write(shootNeutral);

         }

       }

      state = 2;

    }

  }

}

 

 

void writeDegrees(String servoName, int degree){

  // 1000 is 180 degrees; 2000 is 0 degrees; 1500 is 90 degrees

  // every microsecond is .18 degrees

  if(degree == 0){

         //yServo.writeMicroseconds(1000);                                                                                               // can uncomment this to make sentry go back to same location everytime it detects no motion

  }

  if(currentAngleServo2 < degree){                                                                                                                          // rotates servo counterclockwise

    if(degree >= 0 && degree <=180){                                                                                                       // bounds checking

                  // loop to make servo move more smoothly

      for(int i = (1000+(currentAngleServo2/degreesPerSecond)); i <= (1000+(degree/degreesPerSecond)); i++){

          yServo.writeMicroseconds(i);

      }

    }

    updateAngle(servoName,degree);

  }else if(currentAngleServo2 > degree){                                                                                               // rotates servo clockwise

    if(degree >= 0 && degree <=180){

      for(int i = (1000+(currentAngleServo2/degreesPerSecond)); i >= (1000+(degree/degreesPerSecond)); i--){

        //int secondsToWrite = 1000+(i/degreesPerSecond);

          yServo.writeMicroseconds(i);

      }

    }

    updateAngle(servoName,degree);

  }

}

 

// keeps track of degree of y servo

void updateAngle(String servoName, int degree){

currentAngleServo2 = degree;

}

 

//////////////////////////////////////////////////////////////////////////////////////////////////

 

 

 

// Authors: Bob Cole and Mark Carroll

// OpenCV used for motion tracking to control a paintball gun

// Data sent serially to two arduinos: one controlling the x servo and one controlling the y servo and shooting servo

 

#include "stdafx.h"

 

 

#include <stdio.h>

#include <time.h>

#include <cv.h>

#include <cxcore.h>

#include <highgui.h>

#include <iostream>

#include "ofMain.h"

 

ofSerial serialX;

ofSerial serialY;

int shootX;

int shootY;

int xTotal;

int yTotal;

int xCount;

int yCount;

int zero = 0;

 

int main(int argc, char** argv){

       shootX = 0;

       shootY = 0;

       IplImage* frameTime1 = NULL;                                                      // previous frame

       IplImage* frameTime2 = NULL;                                                      // current frame

       IplImage* frameForeground = NULL;

 

       CvCapture* capture = cvCreateCameraCapture(0);                       // sets up capture from camera

 

       serialX.enumerateDevices();                                                       // sets up serial ports

       if(serialX.setup("COM4", 9600) == 1){                                      // change port everytime arduino port changes

              printf("Arduino X-axis successfully connected!!\n");

       }

       serialY.enumerateDevices();

       if(serialY.setup("COM9", 9600) == 1){                                      // change port if needed

              printf("Arduino Y-axis successfully connected!!\n");

       }

 

       int prevX = 0;

       int prevXs[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

       bool showImage = true;

 

       frameTime2 = cvQueryFrame(capture);                                               // get frame from camera

       frameForeground = cvCloneImage(frameTime2);                                // sets the size of frameForeground

       frameTime1 = cvCloneImage(frameTime2);                                     // sets size of frameTime1

 

       while(1){

              frameTime2 = cvQueryFrame(capture);                                        // capture frame

              if(!frameTime2) break;

              if(frameTime1 != NULL){

                     cvAbsDiff(                                                                        // calculates difference of each pixel between current and previous frame

                           frameTime1,

                           frameTime2,

                           frameForeground

                           );

 

                     cvThreshold(                                                         // if the change of a pixel is NOT at least 50, sets that pixel to 255

                           frameForeground,

                           frameForeground,

                           50,

                           255,

                           CV_THRESH_BINARY

                           );

 

 

              xTotal = 0;

              yTotal = 0;

              xCount = 0;

              yCount = 0;

              for(int row = 0; row < frameForeground->width; row = row + 2){                           // loops through every other pixel in the row

                     for(int col = 0; col < frameForeground->height; col = col + 2){                   // loops through every other pixel in the column

                           if(cvGet2D(frameForeground, col, row).val[2] != 0){                               // checks RGB values for 0 (no change)

                                  xCount++;

                                  yCount++;

                                  xTotal += row;

                                  yTotal += col;

                           }else if(cvGet2D(frameForeground, col, row).val[1] != 0){

                                  xCount++;

                                  yCount++;

                                  xTotal += row;

                                  yTotal += col;

                           }else if(cvGet2D(frameForeground, col, row).val[0] != 0){

                                  xCount++;

                                  yCount++;

                                  xTotal += row;

                                  yTotal += col;

                           }

                     }

              }

 

              if(xCount != 0 && yCount != 0 && xCount > 50){                // generates x & y coordinate based on how many x pixels changed

                     shootX = xTotal/xCount;

                     shootY = (yTotal/yCount);

                     if(shootY >= 18){                                                          // limits y range

                           shootY = shootY-18;

                     }

                     int shootXcopy = shootX;

                     //----start of data structure to store previous x coordinates

                     int total = 0;

                     for(int i = 0; i < 19 ; i++){

                           total += (prevXs[i]-prevXs[i+1]);

                     }

                     total = total/19;

                     if((shootX + (10*total)) >= 0 && (shootX + (10*total)) <= 320){

                           shootX = shootX + (10*total);

                     }

 

                     //--end of data structure

 

                     for(int i = 19; i > 0; i--){

                           prevXs[i] = prevXs[i-1];

                     }

 

                     prevXs[zero] = shootXcopy;

                     prevX = shootXcopy;

                     if(shootX > 3 && shootX < 315  && shootY > 3 && shootY < 235){                                  // draws a green box where leading coordinate is

                           for(int i = shootX-3; i < shootX+4; i++){

                                  for(int j = shootY-3; j < shootY + 4; j++){

                                         frameTime2->imageData[j*frameForeground->widthStep + 3*i] = (uchar)0;

                                         frameTime2->imageData[j*frameForeground->widthStep + 3*i + 1] = (uchar)255;

                                         frameTime2->imageData[j*frameForeground->widthStep + 3*i + 2] = (uchar)0;

                                  }

                           }

                     }

 

                     // will change from putting a square on the screen to crosshairs

 

                     // this all needs commented out if the above double for loop stays uncommented

                     /*for(int i = 0; i < frameForeground->width; i++){

                           frameTime2->imageData[shootY*frameForeground->widthStep + 3*i] = (uchar)0;

                           frameTime2->imageData[shootY*frameForeground->widthStep + 3*i + 1] = (uchar)255;

                           frameTime2->imageData[shootY*frameForeground->widthStep + 3*i + 2] = (uchar)0;

                     }*/

                     /*for(int j = 0; j < frameForeground->height; j++){

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xLeft] = (uchar)0;

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xLeft + 1] = (uchar)255;

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xLeft + 2] = (uchar)0;

                     }

                     for(int j = 0; j < frameForeground->height; j++){

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xRight] = (uchar)0;

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xRight + 1] = (uchar)255;

                           frameTime2->imageData[j*frameForeground->widthStep + 3*xRight + 2] = (uchar)0;

                     }*/

              }

 

              serialX.flush();                                              // clears serial

              serialX.writeByte(255);                                       // write start byte

              serialX.writeByte(shootX);                             // write first 8 bits

              shootX>>=8;                                                          // shift 8 bits

              serialX.writeByte(shootX);                             // write next 8 bits

 

              serialY.flush();

              serialY.writeByte(255);

              serialY.writeByte(shootY);

              shootY>>=8;

              serialY.writeByte(shootY);

              int toShoot = 0;

              toShoot = (xCount>50)?1:0;

              serialY.writeByte(toShoot);

 

              shootX = 0;

              shootY = 0;

              if(xCount > 75){                                       // writes shoot byte is enough changes in pixels

                     serialY.writeByte(1);

              }else{

                     serialY.writeByte(0);

              }

 

              if(showImage){                                                                                  // displays frames

                     cvShowImage("Original", frameTime2);

                     cvShowImage("Previous", frameTime1);

                     cvShowImage("frameForeground", frameForeground);

                     char c = cvWaitKey(1);

                     if(c  == 'q'){                                                                    // closes frameTime2 is key 'q' is pressed

                           showImage = false;

                           cvDestroyWindow("Original");

 

                     }

              }

              }

              cvCopy(frameTime2, frameTime1);                                            // copies current frame to previous frame

       }

       cvReleaseImage(&frameTime1);                                                      // releases all the frames and captures

       cvReleaseImage(&frameTime2);

       cvReleaseImage(&frameForeground);

       cvReleaseCapture(&capture);

 

}