Graham Kitteridge
Published © CC BY-NC

Omni-Directional People Tracking Friendly Robot

This omni-directional robot will detect your face, track you and then close in on his prey. Autonomously.

IntermediateFull instructions provided10 hours10,856
Omni-Directional People Tracking Friendly Robot

Things used in this project

Hardware components

L298N Dual Stepper Motor Driver Controller Board Module
Dual motor driver capable of up to 2A o drive the motors
×2
LM2596 DC DC Switching Adjustable Step Down Voltage Regulator
To step down the 12V from the battery to a 5V Rail
×1
Onboard Lipo Alarm Battery Checker Low Voltage Detector RC Plane Quadcopter
Used to sound when the lipo is getting low on voltage to ensure it is not broken during use.
×1
24RPM DC 12V 4mm Shaft Geared Motor
Three for three wheels
×3
Arduino Nano R3
Arduino Nano R3
Motor controller
×1
Microsoft LifeCam HD-3000 Web camera
For detecting peoples faces, you can use a cheaper camera but this is the one I had in the office.
×1
Jumper wires (generic)
Jumper wires (generic)
Lots of jumper wires for power management and signal useage
×10
Creator Ci20
Creator Ci20
How could we forget the brains of the operation, the Ci20!
×1
11.1V 3Cell Lipo Battery
Example to buy from ebay attached.
×1

Software apps and online services

OpenCV
OpenCV

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)
Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Custom parts and enclosures

Base of Robot

This is the base of the omnidirectional robot, designed by me to fit all the parts in and have a lid attached

Lid of Robot

This is the lid which shows off the Ci20 in all it's glory.

Omni Directional Wheels

This is the original design of the omni-directional wheels. I used one half of this with the rims and then edited it to include the motor shaft

Wheel with shaft

A remixed wheel from the thingiverse link

Schematics

Schematic and "Wiring Diagram"

See better description in story section

Code

Arduino Sketch - Main

Arduino
This Arduino code receives the drive command from the Ci20 over serial and ensures that the motors are driven in the correct fashion to go in the right direction.

I originally wrote this for a Raspberry Pi but it works with the Ci20 aswell
// Motor 1
int dir1PinA = 3;
int dir2PinA = 2;
int speedPinA = 9; // Needs to be a PWM pin to be able to control motor speed

// Motor 2
int dir1PinB = 4;
int dir2PinB = 5;
int speedPinB = 8; // Needs to be a PWM pin to be able to control motor speed

// Motor 3
int dir1PinC = 6;
int dir2PinC = 7;
int speedPinC = 10; // Needs to be a PWM pin to be able to control motor speed

int x = 0;
int y = 0;
int dominantUltrasonic = 0;
bool moveMotor = false;

int startTime = 0;

void setup() {
  Serial.begin(9600);
  pinMode(dir1PinA,OUTPUT);
  pinMode(dir2PinA,OUTPUT);
  pinMode(speedPinA,OUTPUT);
  pinMode(dir1PinB,OUTPUT);
  pinMode(dir2PinB,OUTPUT);
  pinMode(speedPinB,OUTPUT);
  pinMode(dir1PinC,OUTPUT);
  pinMode(dir2PinC,OUTPUT);
  pinMode(speedPinC,OUTPUT);
  startTime = millis();
}

String rpiString;

void loop() {
    
  readDataFromRPi();
  

  // This tests whether we have received a value from the RPi in the last second.
  // It effectively acts as buffer so that it doesn't keep stopping & starting as the move command is not continuous
  // It also allows stoppage if nothign is coming in from the RPi
//  int  elapsedTime =   millis() - startTime; 
//  if (elapsedTime > 1000)
//  {
//    x = 0;
//    y = 0;
//    dominantUltrasonic = 0;
//    moveMotor = false;
//    startTime = millis();
//    
//  }

  // Send the X & Y to the motors
  //if(moveMotor == true && (x != 0 && y != 0))
  //{
    //Serial.println("MovingMotor");
   // driveInDirection(x,y);  
  //}
  //if(x == 0 && y == 0)
  //{
    //Serial.println("ZeroMMotor");
   // driveInDirection(x,y);    
  //} 
  
         
  // This might just be left over from the ultrasonics - hesitant to remove
  delay(30);
}


void readDataFromRPi()
{
   

 // Read from Rpi
  while (Serial.available()) 
  {
    delay(3);  //delay to allow buffer to fill
    if (Serial.available() >0) 
    {
      char c = Serial.read();  //gets one byte from serial buffer
      rpiString += c; //makes the string readString
      if(c == 'n')
      {
        break;
      }
    }     
  } // ENDWHILE

  
  // If something has been read from the RPi then put it into x,y & domniantUltrasonic
  if (rpiString.length() >0) 
  {
      Serial.println(rpiString); //see what was received

      String isRotate = getValue(rpiString, ' ',0);
     

      String xval = getValue(rpiString, ' ', 1);
      String yval = getValue(rpiString, ' ', 2);

      x = xval.toInt();
      y = yval.toInt();
      startTime = millis();

       if (isRotate == "r")
      {
          rotate(x);
      }
      else
      {
        driveInDirection(x,y);  
      }

    
      
      
      
      rpiString="";
  } //ENDIF
}



 String getValue(String data, char separator, int index)
{
 int found = 0;
  int strIndex[] = {0, -1  };
  int maxIndex = data.length()-1;
  for(int i=0; i<=maxIndex && found<=index; i++){
  if(data.charAt(i)==separator || i==maxIndex){
  found++;
  strIndex[0] = strIndex[1]+1;
  strIndex[1] = (i == maxIndex) ? i+1 : i;
  }
 }
  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}

Arduino Sketch - MotorControl

Arduino
This goes with the main ino file and needs including in the same project file
/* 
 *  Motor control code
 *  
 *  This class will include code to make the robot go in any direction 
 *  and rotate around the center point.
 */

void driveInDirection(float newX, float newY)
{
  delay(20);
  
  float x = newX;
  float y = newY;
  
  float theta = atan2(y,x);
  float mag = sqrt((x*x) + (y*y));
  float vx = mag * cos(theta);
  float vy = mag * sin(theta);
  
  float w1 = -vx;
  float w2 = 0.5 * vx - sqrt(3)/2 * vy;
  float w3 = 0.5 * vx + sqrt(3)/2 * vy;
  
  // Get largest w value
  float wSet[] = {w1, w2, w3};
  float largestValue = 0.0;
  
  for (int i = 0; i < 3; i++)
  {
    if(abs(wSet[i]) > largestValue)
    {
        largestValue = abs(wSet[i]);
    }
  }
  
  float speedCoef = (float)147.0 / largestValue;
  
  w1 = w1 * speedCoef;
  w2 = w2 * speedCoef;
  w3 = w3 * speedCoef;   

  if (x ==0 && y == 0)
  {
      w1 = 0;
      w2 = 0;
      w3 = 0;
  }
  
  Serial.println(w1);
  Serial.println(w2);
  Serial.println(w3);
  
  w1 = constrain(w1, -150, 150);
  w2 = constrain(w2, -150, 150);
  w3 = constrain(w3, -150, 150);
  
  boolean w1_ccw = w1 < 0 ? true : false;
  boolean w2_ccw = w2 < 0 ? true : false;
  boolean w3_ccw = w3 < 0 ? true : false;
  
  byte w1_speed = (byte) map(abs(w1), 0, 150, 0, 255);
  byte w2_speed = (byte) map(abs(w2), 0, 150, 0, 255);
  byte w3_speed = (byte) map(abs(w3), 0, 150, 0, 255);
  
  printMotorSpeed(w1_speed, 1);
  printMotorSpeed(w2_speed, 2);
  printMotorSpeed(w3_speed, 3);
  
  analogWrite(speedPinA, w1_speed);//Sets speed variable via PWM 
  analogWrite(speedPinB, w2_speed);
  analogWrite(speedPinC, w3_speed);//Sets speed variable via PWM 
  
  digitalWrite(dir1PinA, !w1_ccw);
  digitalWrite(dir2PinA, w1_ccw);
    
  digitalWrite(dir1PinB, !w2_ccw);
  digitalWrite(dir2PinB, w2_ccw);
  
  digitalWrite(dir1PinC, w3_ccw);
  digitalWrite(dir2PinC, !w3_ccw);
}

void rotate(float milliseconds)
{
 float  w1 = 255;
  float w2 = 255;
  float w3 = 255;
  
  boolean w1_ccw = w1 < 0 ? true : false;
  boolean w2_ccw = w2 < 0 ? true : false;
  boolean w3_ccw = w3 < 0 ? true : false;
  
  byte w1_speed = (byte) map(abs(w1), 0, 150, 0, 255);
  byte w2_speed = (byte) map(abs(w2), 0, 150, 0, 255);
  byte w3_speed = (byte) map(abs(w3), 0, 150, 0, 255);
  
  printMotorSpeed(w1_speed, 1);
  printMotorSpeed(w2_speed, 2);
  printMotorSpeed(w3_speed, 3);
  
  analogWrite(speedPinA, w1_speed);//Sets speed variable via PWM 
  analogWrite(speedPinB, w2_speed);
  analogWrite(speedPinC, w3_speed);//Sets speed variable via PWM 
  
  digitalWrite(dir1PinA, !w1_ccw);
  digitalWrite(dir2PinA, w1_ccw);
    
  digitalWrite(dir1PinB, !w2_ccw);
  digitalWrite(dir2PinB, w2_ccw);
  
  digitalWrite(dir1PinC, w3_ccw);
  digitalWrite(dir2PinC, !w3_ccw);
  
  delay(milliseconds);
  
  analogWrite(speedPinA, 0);//Sets speed variable via PWM 
  analogWrite(speedPinB, 0);
  analogWrite(speedPinC, 0);//Sets speed variable via PWM 

}

void printMotorSpeed(byte motorSpeed, int motor)
{
    Serial.print("Motor");
    Serial.print(motor);
    Serial.print(": ");
    Serial.println(motorSpeed); 
}

FaceTracking C++ For Ci20

C/C++
Follow guide in Story
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <iostream>
#include <stdio.h>

using namespace std;
using namespace cv;

CascadeClassifier face_cascade, eyes_cascade;
String window_name = "Face Detection";

#include <termios.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>


int sendSerial(char* message)
{
int fd = open("/dev/ttyUSB0", O_RDWR);
if (fd == -1)
{
    perror("dev/ttyUSB0");
    return 1;
}

struct termios tios;
tcgetattr(fd, &tios);

tios.c_iflag = IGNBRK | IGNPAR;
tios.c_oflag = 0;
tios.c_lflag = 0;
cfsetspeed(&tios, B9600);
tcsetattr(fd, TCSAFLUSH,&tios);

usleep(1000);

//char msg[] = "50 50";
write(fd, message, strlen(message));

return  0;
}


/**
 * Detects faces and draws an ellipse around them
 */

void detectFaces(Mat frame) {

  std::vector<Rect> faces;
  Mat frame_gray;

  // Convert to gray scale
  cvtColor(frame, frame_gray, COLOR_BGR2GRAY);

  // Equalize histogram
  equalizeHist(frame_gray, frame_gray);

  // Detect faces
  face_cascade.detectMultiScale(frame_gray, faces, 1.1, 3,
				0|CASCADE_SCALE_IMAGE, Size(30,30));

  // Iterate over all of the faces
  for(size_t i = 0; i < faces.size(); i++) {

    // Find center of faces
    Point center(faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2);

    Mat face = frame_gray(faces[i]);
    std::vector<Rect> eyes;

    // Try to detect eyes, inside each face
   // eyes_cascade.detectMultiScale(face, eyes, 1.1, 2,
	//			  0 |CASCADE_SCALE_IMAGE, Size(50, 50) );

 //   if(eyes.size() > 0)
      // Draw ellipse around face
      ellipse(frame, center, Size(faces[i].width/2, faces[i].height/2),
	      0, 0, 360, Scalar( 255, 0, 255 ), 4, 8, 0 );
		  
	if(center.x > frame.cols/3 && center.x < frame.cols/3*2)
	{
		char msg[] = "F 50 50";
		sendSerial(msg);
	}
	else
	{
		char msg[] = "0 0 ";
		sendSerial(msg);
	}
		
	
  }

  // Display frame
  imshow( window_name, frame );
}


int main() {
	
	char msg[] = "0 0 ";
	sendSerial(msg);
	

  VideoCapture cap(0); // Open default camera
  cap.set(CV_CAP_PROP_FRAME_WIDTH, 480);
  cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
  Mat frame;

  if(!face_cascade.load("haarcascade_frontalface_default.xml"))
  {
	std::cout << "classifier";
	return -1;
  } // load faces
 // eyes_cascade.load("haarcascade_eye_tree_eyeglasses.xml"); // load eyes

  while(cap.read(frame)) {
    detectFaces(frame); // Call function to detect faces
    if( waitKey(30) >= 0)    // pause
      break;
  }
  return 0;
}

Credits

Graham Kitteridge

Graham Kitteridge

1 project • 8 followers

Comments

Related channels and tags
  • Arduino
  • Creator
  • Microsoft
  • OpenCV
  • Robotics