Xbox Controlled Camera Bot


Pi 4/ LPC1768 bot with Pi camera controlled by Xbox controller

You are viewing an older revision! See the latest version

xbox controlled camera bot

Xbox Controlled Camera Bot

Team Members:

  • Nikhil Patel
  • Taia Modlin
  • Loi Mac
  • Timothy Pierce

Parts List:

Project Description

This project is a camera mounted shadow chassis robot. It has the following features:

  • Bluetooth Xbox One controller control
    • Left/right joystick y-axis controls motors and D-pad controls pan/tilt
  • Hall effect sensor encoders to sync motor speeds
  • Raspberry Pi 4 Model B/2GB and Mbed LPC1768 used for computation and control
    • Xbox controller connects to Pi via bluetooth
    • Pi interprets and sends controller state to Mbed virtual COM port
    • Mbed controls devices based on controller state and syncs motors
  • Pi camera mounted on a pan/tilt module for greater viewing range
    • Streams feed over WiFi locally from Pi
  • Bi-directional motors (3 states for each - forwards, reverse, and stop)

Wiring

mbedhbridgeleft motorright motorDC battery pack
VOUTVCC
VM4.8V to 6V
gndgnd
AO1red
AO2black
BO1red
BO2black
p23PWMA
p24PWMB
p30AI1
p29AI2
p5BI1
p6BI2
VOUTSTBY
mbedtop servobottom servoDC battery pack
gndgnd (brown)gnd (brown)gnd
power (red)power (red)4.8V to 6V
p25signal (orange)
p26signal (orange)
mbedleft motor encoderright motor encoder
VUpower (red)power (red)
gndgnd (black)gnd (black)
p22signal (white)
p21signal (white)

Note

The Raspberry Pi was powered by USB from a 5V 3A portable cell phone charger. The Mbed was powered by USB from the Raspberry Pi.

Diagram

Xbox Controller to Raspberry Pi Input

Technical Details

To connect the xbox controller to the spy robot, the xbox controller communicates with the Raspberry Pi over bluetooth and then the Raspberry Pi sends the xbox controller inputs to the mbed via the COM port. To connect the xbox controller to the Raspberry Pi the xpadneo xbox controller drivers were used. More information on these drivers and installation instructions can be found here: https://github.com/atar-axis/xpadneo. To read the controller inputs, a C joystick API is used. More information on this API can be found here: https://www.kernel.org/doc/Documentation/input/joystick-api.txt.

This code reads the left and right joysticks of the controller as well as the directional pad. For each user input two characters are assigned: a character to denote the action of the input and a character to denote the direction of movement. ‘(’ represents an action for the left motor, ‘)’ represents an action for the right motor, and ‘!’ represents an action for the camera servo. For the second character denoting direction, the values have different meanings for the wheel motors and for the camera servo. For the wheel motors ‘0’ = stop, ‘1’ = forward, and ‘2’=reverse. For the camera servo, ‘1’ = left, ‘2’ = right, ‘3’ = up, ‘4’ = down, and ‘0’ = stop. The code for the xbox controller input is below.

controllerPiToMbed.cpp

 
#include <stdio.h>
#include <linux/joystick.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>

/*
 * This function will check that events are happening on the controller
 * to make sure the controller is connected.
 *  
 */
int controllerConnected(int fd, struct js_event *event)
{
    
    if (sizeof(*event) == read(fd, event, sizeof(*event))){
        return 1;
    }

    return -1;
}

//This struct stores the tag and movement data for each joystick action
struct botAction {
	char tag,movement;
};

//This struct stores the previous movement data for each joystick action
struct previousBotAction {
	char prevMovement;
};

/*
 * This function reads the controller input event, determines which joystick it is on, and determines which direction it is
 * For the left joystick the tag is '('
 * For the right joystick the tag is ')'
 * For the wheel motor movements 0=stop 1=forward and 2=reverse
 * 
 * For the directional pad controlling the camera direction, the tag is '!'
 * The servo movements are 0=stop, 1=left, 2=right, 3=up, and 4=down
 */
size_t controllerOutput(struct js_event *event, struct botAction action[7], struct previousBotAction prevAction[7])
{
	size_t joystick = event->number;
	prevAction[joystick].prevMovement = action[joystick].movement;

	if(joystick == 1){
		action[joystick].tag = '(';
		if(event->value > -2000 && event->value < 2000){
			action[joystick].movement = '0';
		}else if(event->value > 2000){
			action[joystick].movement = '2';
		} else if(event->value < -2000){
			action[joystick].movement = '1';
		} 
	} else if (joystick == 4) {
		action[joystick].tag = ')';
		if(event->value > -2000 && event->value < 2000){
			action[joystick].movement = '0';
		}else if(event->value > 2000){
			action[joystick].movement = '2';
		} else if(event->value < -2000){
			action[joystick].movement = '1';
		} 
	} else if (joystick == 6) {
		action[joystick].tag = '!';
		if(event->value > -2000 && event->value < 2000){
			action[joystick].movement = '0';
		}else if(event->value > 2000){
			action[joystick].movement = '2';
		} else if(event->value < -2000){
			action[joystick].movement = '1';
		} 
	} else if (joystick == 7) {
		action[joystick].tag = '!';
		if(event->value > -2000 && event->value < 2000){
			action[joystick].movement = '0';
		}else if(event->value > 2000){
			action[joystick].movement = '4';
		} else if(event->value < -2000){
			action[joystick].movement = '3';
		} 
	} 
	
	return joystick;
}

/*
 * This is the main function that will call the above functions and send the data 
 * to the MBED via the COM Port
 */
int main()
{
	struct js_event event;
	struct botAction action[7] = {0};
	struct previousBotAction prevAction[7] = {0};
	size_t joystick;
	int n;
	
	//Open the bluetooth port with the joystick
	int fdBT = open("/dev/input/js2", O_RDONLY);
	
	if(fdBT == -1){
		printf("No Joystick Found");
		return -1;
	}
	
	//Open the serial port with the mbed
	int fdCP = open("/dev/ttyACM0", O_RDWR | O_NOCTTY | O_NDELAY);
	
	if (fdCP == -1) {  
		perror("open_port: Unable to open /dev/ttyACM0 - ");
		return(-1);
	}
	
	//Set the serial port settings
	struct termios options; 
	tcgetattr(fdCP,&options);   //Get current serial settings in structure
	cfsetspeed(&options, B9600);   //Change a only few
	options.c_cflag &= ~CSTOPB;
	options.c_cflag |= CLOCAL;
	options.c_cflag |= CREAD;
	cfmakeraw(&options);
	tcsetattr(fdCP,TCSANOW,&options);    //Set serial to new settings
	sleep(1);
	
	//Main loop to get the controller data and send it to the MBED
	while(controllerConnected(fdBT, &event) == 1)
	{
		if(event.type == JS_EVENT_AXIS){
			joystick = controllerOutput(&event, action, prevAction);
			if(joystick == 1 || joystick == 4 || joystick == 6 || joystick == 7)
			{
				if(prevAction[joystick].prevMovement != action[joystick].movement)
				{
					printf("%c%c\n", action[joystick].tag, action[joystick].movement);
					// Write to the port - just the tag
					n = write(fdCP,&action[joystick].tag,1);
					if (n < 0) {
						perror("Write failed - ");
						return -1;
					}
					
					//Write to the port - just the movement
					n = write(fdCP,&action[joystick].movement,1);
					if (n < 0) {
						perror("Write failed - ");
						return -1;
					}
				}
			}
		} 
		fflush(stdout);
	}
	
	close(fdBT);
	tcdrain(fdCP);
	close(fdCP);
	return 0;
}

 

Spycam

Technical Details

The Spycam is a live video feed from the Pi Camera to the browser at <Pi IPv4>:8000. We used this C/C++ socket tutorial and this HTTP Server/Client tutorial for help getting the basic server started. We also used the Python Pi Camera API for direction on how web streaming works. We used a C++/OpenCV version of the original C Pi Camera library. This library, raspicam allowed us to start and use the camera for video with standard Pi Camera functions. The OpenCV component added the ability to easily encode each frame as a JPEG image.

How it works

Once the server is started and listening for connections, it opens the camera and lets it warm up. After the camera is ready, it will wait for a web client to connect. Once they are connected, the server will send the initial HTTP header that tells the client it will receive a stream of data and to replace each frame with the subsequent one. After that the server starts grabbing frames, encoding them into JPEGs, reformating those JPEGs so that they can be sent over the connection, creating and sending a header for each JPEG, and finally sending the JPEG. The effect in the browser is a video without sound.

Installation

  1. Make sure to have OpenCV installed on your Pi
  2. Following the installation instructions on the raspicam page, install raspicam in the same directory as SpycamServer.cpp
  3. Compile SpycamServer with the following command and run it with ./SpycamServer

g++ SpycamServer.cpp -o SpycamServer -L/opt/vc/lib -I/usr/local/include/opencv4 -I/usr/local/include -lraspicam -lraspicam_cv -lmmal -lmmal_core -lmmal_util -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_imgcodecs 

To get the live feed, first start the server, then open any browser and go to <Pi IPv4>:8000.

SpycamServer.cpp

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <iostream>

#include <fstream>
#include <raspicam/raspicam_cv.h>

#define PORT 8000
#define BUFFSIZE 10000

int main(void) {
	int sockfd, new_fd;
	struct sockaddr_in address;
	int addrlen = sizeof(address);
	int yes = 1;

	raspicam::RaspiCam_Cv Camera;
	cv::Mat image;
	
	//get socket
	if ((sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == 0) {
		perror("socket\n");
		exit(1);
	}
	
	setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int)); //reuse address so address already in use errors
	address.sin_family = AF_INET; //IPv4
	address.sin_addr.s_addr = INADDR_ANY; //0.0.0.0 - listen on all available interfaces
	address.sin_port = htons(PORT); //converting to network byte order
	
	memset(address.sin_zero, '\0', sizeof address.sin_zero);

	//bind to socket
	if(bind(sockfd, (struct sockaddr *)&address, sizeof(address))<0) {
		perror("bind error\n");
		exit(1);
	}

	// start listening for connections
	if (listen(sockfd, 0) <0) {
		perror("listen error\n");
		exit(1);
	}


	printf("Opening camera...\n");
	Camera.set(cv::CAP_PROP_FORMAT, CV_8UC3); //set to RGB
	Camera.setVerticalFlip(true); //camera is upside-down on bot
	Camera.set(cv::CAP_PROP_FPS, 30); //getting 30 frames/sec
	if (!Camera.open()) {
		printf("Error opening camera\n");
		exit(1);
	}
	sleep(3); //waiting for camera to warm up
	printf("Capturing...\n");
	
	//get connection
	if ((new_fd = accept(sockfd, (struct sockaddr *)&address, (socklen_t*)&addrlen))<0) {
		printf("accept error\n");
		exit(1);
	}
	
	printf("connection accepted...\n");	
		
	/*
	 * This header initializes streaming
	 * multipart/x-mixed-replace basically says that a bunch of frames
	 * are coming and it will replace each frame with the other. The 
	 * effect with images is a video.
	 */
	printf("sending header...\n");
	char* head = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=FRAME\r\n\r\n";
	if(write(new_fd, head, strlen(head)) == -1) {
		printf("error send: header\n");
		exit(1);
	}
	printf("starting stream...\n");
	
	while(1) {
		Camera.grab(); //grabs a frame from camera
		Camera.retrieve(image); //places grabbed frame in given buffer


		std::vector<uchar> buff; //resizable buffer
		std::vector<int> param(2); //specify how jpeg should be encoded
		param[0] = cv::IMWRITE_JPEG_QUALITY;
		param[1] = 70;
		//encodes image to jppeg format
		//resizes given buffer to fit encoded image
		// places encoded image in buffer
		cv::imencode(".jpg", image, buff, param);
		
		//need to change image's data type so it can be sent
		const char* frame = reinterpret_cast<const char *>(buff.data());

		//send a header stating a jpeg is sent and its size for each image
		char buffbody[BUFFSIZE];
		snprintf(buffbody, BUFFSIZE -1, "--FRAME\r\nContent-Type: image/jpeg\r\nContent-Length: %lu\r\n\r\n", buff.size());
		
		if (write(new_fd, buffbody, strlen(buffbody)) <= 0) {
			printf("Failed to write header. Closing down server...\n");
			break;
		}
		
		
		// send image
		if (write(new_fd, frame, buff.size()) <= 0) {
			printf("Failed to send jpeg. Closing down server...\n");
			break;
		}
		
		
	}
	Camera.release();	//free camera resources
	close(new_fd); //close client connection
	close(sockfd); //close socket
	printf("connection closed\n");
	return 0;
}

Mbed-Raspberry-Pi Interpreter

The interpreter takes in characters received from the Serial port and updates what the Mbed sees as the current state of the controller. The Raspberry Pi sends over 3 characters ‘!’ , ‘(‘ , and ‘)’ depending on whether it wants to tilt the camera, pan it, or move the motors. The ‘!’ character represents button presses on the D pad which control the pan and tilt. The characters ‘(‘ and ‘)’ represent the left and right joystick respectively on the Xbox controller. We try to balance out the speed of the left and right motors using the SpeedBalance() thread. This thread takes into account the encoder attached to the motors. Whenever both motors are meant to be moving in the same direction, the program starts to count the encoders. As long as it keeps moving straight, the encoder count will be checked each loop to see if the speed difference is beyond the tolerance point. Once it is, we start decreasing the speed of whichever motor is moving faster. We’ve drawn most of the inspiration for handling the inputs from the pi from the mbed wiki page describing utilization of the adafruit bluefruit LE UART Friend. This is done in the main thread. We also set up 2 additional threads for handling motor movements and the camera’s tilt/pan movement. Again, the resource referenced for this comes from the RTOS thread wiki. Use of this code requires that libraries be imported from the mbed website. These include the Servo, Motor, and RTOS (OS 2) libraries

Mbed Code

main.cpp

#include "mbed.h"
#include "Servo.h"
#include "Motor.h"
#include "rtos.h"

Serial pi(USBTX, USBRX);
InterruptIn leftEncoder(p22);
InterruptIn rightEncoder(p21);
Motor leftMotor(p23, p30, p29);
Motor rightMotor(p24, p5, p6);
Servo topServo(p25);
Servo bottomServo(p26);

volatile int xpadState = 0; //left-right Dpad
volatile int ypadState = 0; //up-down Dpad
volatile int leftJoystickState = 0;
volatile int rightJoystickState = 0;
volatile int leftEncoderCount = 0; //keep track of encoder ticks
volatile int rightEncoderCount = 0;

//increment left encoder counts
void LeftCount()
{
	leftEncoderCount = leftEncoderCount + 1;
}

//increment right encoder counts
void RightCount()
{
	rightEncoderCount = rightEncoderCount + 1;
}

//rotate the pan/tilt servos
void RotateServos()
{
	float topServoPos = 0.0;
	float bottomServoPos = 0.0;
    
	while(1){
	if(xpadState == -1) //pan right
	{
    	bottomServoPos = bottomServoPos + 0.05;
    	if(bottomServoPos > 1.0) bottomServoPos = 1.0;
	}
	else if(xpadState == 1) //pan left
	{
    	bottomServoPos = bottomServoPos - 0.05;
    	if(bottomServoPos < 0.0) bottomServoPos = 0.0;
	}
    
	if(ypadState == 1) //tilt up
	{
    	topServoPos = topServoPos + 0.05;
    	if(topServoPos > 1.0) topServoPos = 1.0;
	}
	else if(ypadState == -1) //tilt down
	{
    	topServoPos = topServoPos - 0.05;
    	if(topServoPos < 0.0) topServoPos = 0.0;
	}
    
	topServo = topServoPos;
	bottomServo = bottomServoPos;
	Thread::wait(150);
	}
}


//Sets and keeps motor speed matched using encoder output
void SpeedBalance()
{
	bool interruptAttached = false; //flag indicating whether encoders are being counted
	bool speedSet = false; //flag indicating if the initial motor speed has been set before   switching to encoder control
    
	//used when speed adjusting using encoders
	float leftMotorSpeed = 0.0;
	float rightMotorSpeed = 0.0;
    
	int encoderCountDifference = 0;
	const int countDifferenceTolerance = 8; //How many counts difference between encoders is allowed (8 per rotation)
    
	while(1){
	if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
	{
    	if(speedSet == false) //set motor speed initally
    	{
        	leftMotorSpeed = leftJoystickState;
        	rightMotorSpeed = -1*rightJoystickState;
        	speedSet = true;
    	}
   	 
    	//start counting encoders if not already
    	if(interruptAttached == false)
    	{
        	leftEncoder.fall(&LeftCount);
        	rightEncoder.fall(&RightCount);
        	interruptAttached = true;
    	}
   	 
    	encoderCountDifference = leftEncoderCount - rightEncoderCount;
    	if(encoderCountDifference > countDifferenceTolerance) //if left encoder counted more, left motor is faster
    	{
        	//right motor stays constant, left motor slows down to follow it
        	rightMotor.speed(-1*rightJoystickState);
       	 
        	//add or subtract motor speed, depending on if its positive or negative
        	if(leftMotorSpeed < 0){
            	leftMotorSpeed = leftMotorSpeed + 0.001;
            	leftMotor.speed(leftMotorSpeed);
        	}
        	else if(leftMotorSpeed > 0){
            	leftMotorSpeed = leftMotorSpeed - 0.001;
            	leftMotor.speed(leftMotorSpeed);
        	}
        	leftEncoderCount = 0; //reset encoder counts
        	rightEncoderCount = 0;
        	Thread::yield();
    	}
    	else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
    	{
        	//left motor speed stays constant, right motor slows down to follow it
        	leftMotor.speed(leftJoystickState);
       	 
        	//add or subtract motor speed, depending on if its positive or negative
        	if(leftMotorSpeed < 0){
            	rightMotorSpeed = rightMotorSpeed + 0.001;
            	rightMotor.speed(rightMotorSpeed);
        	}
        	else if(leftMotorSpeed > 0){
            	rightMotorSpeed = rightMotorSpeed - 0.001;
            	rightMotor.speed(rightMotorSpeed);
        	}
        	leftEncoderCount = 0; //reset encoder counts
        	rightEncoderCount = 0;
        	Thread::yield();
    	}
    	else //if counts within tolerances, keep speeds the same
    	{
        	rightMotor.speed(rightMotorSpeed);
        	leftMotor.speed(leftMotorSpeed);
        	Thread::yield(); //if counts are same, nothing left to do
    	}
	}
	else //The case where joysticks arent in the same direction
	{
    	//set motors using joystick states, since motor speed wont need to be dynamically changed in this case
    	leftMotor.speed(leftJoystickState);
    	rightMotor.speed(-1*rightJoystickState);
    	speedSet = false;
   	 
    	//Stop counting encoders because they aren't being used
    	if(interruptAttached == true)
    	{
        	leftEncoder.fall(NULL);
        	rightEncoder.fall(NULL);
        	interruptAttached = false;
        	leftEncoderCount = 0;
        	rightEncoderCount = 0;
    	}
    	Thread::yield();
	}
	}
}

int main()
{
	Thread motorControl(SpeedBalance);
	Thread servoControl(RotateServos);
    
	char servoNum = '0';
	char leftMotNum = '0';
	char rightMotNum = '0';
	char initialCheck = '0';
    
	while(1)
	{
    	if(pi.readable()) //check if new command from pi available
    	{
        	initialCheck = pi.getc();
        	if (initialCheck == '!' ) { //look for dpad update
            	servoNum = pi.getc();
            	if (servoNum == '0') { //dpad released
                	xpadState = 0;
                	ypadState = 0;
            	}
            	else if (servoNum == '1') { //dpad left
                	xpadState = -1;
            	}
            	else if (servoNum == '2') { //dpad right
                	xpadState = 1;
            	}
            	else if (servoNum == '3') { //dpad up
                	ypadState = 1;
            	}
            	else if (servoNum == '4') { //dpad down
                	ypadState = -1;
            	}
        	}
        	else if (initialCheck == '(') { //look for left joystick update
            	leftMotNum = pi.getc();
            	if (leftMotNum == '0') { //joystick center
                	leftJoystickState = 0;
            	}
            	else if (leftMotNum == '1') { //joystick up
                	leftJoystickState = 1;
            	}
            	else if (leftMotNum == '2') { //joystick down
                	leftJoystickState = -1;
            	}
        	}
        	else if (initialCheck == ')' ) { //look for right joystick update
            	rightMotNum = pi.getc();
            	if (rightMotNum == '0') { //joystick center
                	rightJoystickState = 0;
            	}
            	else if (rightMotNum == '1') { //joystick up
                	rightJoystickState = 1;
            	}
            	else if (rightMotNum == '2') { //joystick down
                	rightJoystickState = -1;
            	}
        	}
    	}
	}
}

Import programCamera_Bot

Mbed-side code for control of the Xbox controlled camera bot.

Demo

Presentation


All wikipages