Revision 2 of Code

The first revision worked well but I knew I could make it better. To do that I decided to make the robot look and turn at 45 degree increments instead of 90 like the previous revision. I added functions that did the turning and looking, but I needed to change how the robot chose which path to take. The previous code it had two choices left or right, revision 2 has four. I wrote a function that found the lowest variable (further distance away) and turned accordingly. Revision two is posted below.

//=======================================================================
// Name        : FInal_Rev2.cpp
// Author      : George
// Version     : Revision 2
// Copyright   : None
// Description : Adding looking while going forward, also adding more turns
//=======================================================================

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <algorithm>
using namespace std;

////////////////////////////////////////////////////////////////////////
/////////       Motor File Paths "can be put in header file"
////////////////////////////////////////////////////////////////////////
FILE *MC = NULL;
char *M1Period = "/sys/devices/ocp.2/pwm_test_P8_13.10/period"; //P8_13 Left Motor
char *M1Duty = "/sys/devices/ocp.2/pwm_test_P8_13.10/duty";     //P8_13 Left Motor
char *M2Period = "/sys/devices/ocp.2/pwm_test_P8_34.11/period"; //P8_34 Right Motor
char *M2Duty = "/sys/devices/ocp.2/pwm_test_P8_34.11/duty";     //P8_34 Right Motor
char *M3Period = "/sys/devices/ocp.2/pwm_test_P9_21.12/period"; //P9_21 Micro Servo
char *M3Duty = "/sys/devices/ocp.2/pwm_test_P9_21.12/duty";     //P9_21 Micro Servo


////////////////////////////////////////////////////////////////////////
/////////       Declared Functions  "can be put in header file"
////////////////////////////////////////////////////////////////////////
void GPIO_46();
void GPIO_47();
void ADC_setup();
int distance();
int Turn(int,int,int,int);
void setup();
void MotorP(int);
void MotorLL(int);
void MotorRR(int);
void MotorEyes(int);
void Look_Right();
void Look_ShortRight();
void Look_TinyRight();
void Look_Left();
void Look_ShortLeft();
void Look_TinyLeft();
void Look_Ahead();
void reboot();
void Push2Start();
void GoStraight();
void TurnRight();
void ShortRight();
void TurnLeft();
void ShortLeft();
void Stop();

int main(int argc, char** argv)
{
int E,NE,W,NW,N,check_dist;
setup();                   // Setting pins, modes, and motors
printf(" You can Push De Button!! \n");
Push2Start();          // Waiting until button is pressed to proceed
int iteration=0;       // counter

while(1){
                  ////////// Added looking while moving
if (iteration < 20) Look_TinyLeft();    
if (iteration > 20) {                            
if (iteration < 40){                  
Look_Ahead();              
}                                          
else Look_TinyRight();      
}                                                
if (iteration == 61) iteration = 0;
                 //////////
GoStraight();
check_dist = distance();
printf("  distance = %d  \n", check_dist);

if ( check_dist >= 1200){         /////  Additional cases were added
Stop();                                     /////   Stop and start looking
for (int ii=1; ii<6; ii++){      /////    Look both ways before crossing

switch(ii){
case 1:
Look_Left();
usleep(400000);
W=distance();
break;
case 2:
Look_ShortLeft();
usleep(400000);
NW=distance();
break;
case 3:
Look_Ahead();
usleep(400000);
N=distance();
break;
case 4:
Look_ShortRight();
usleep(400000);
NE=distance();
break;
case 5:
Look_Right();
usleep(400000);
E=distance();
Look_Ahead();
break;
}
}
printf("    West  = %d  \n", W);
printf("  N West  = %d  \n", NW);
printf("    North = %d  \n", N);
printf("  N East  = %d  \n", NE);
printf("    East  = %d  \n", E);

int num = Turn(W,NW,NE,E);   // finding lowest value
printf("Lowest number is: %d \n", num);

if (W == num) TurnLeft();   //// Compares to lowest value found
if (NW == num) ShortLeft();
if (NE == num) ShortRight();
if (E == num) TurnRight();
}
reboot(); // Checking if reset button is pressed
iteration++;
}
return 0;
}

///////////////////////////////////////////////////////////////////////////////////
////////////// Setup Function
///////////////////////////////////////////////////////////////////////////////////

void setup(){

GPIO_46();         // Exporting and setting PinMode (P8_16)
usleep(20);
GPIO_47();       // Exporting and setting PinMode (P8_15)
usleep(20);
ADC_setup();   // Turning Analog pin 5 on
usleep(20);
MotorP(20000000);  // Setting period for all motors
usleep(20);
Stop();             // Motors in stopped position
usleep(20);
Look_Ahead();      // IR Looking forward
usleep(20);

}

/////////////////////////////////////////////////////////////////////////
///////////// Button Functions
/////////////////////////////////////////////////////////////////////////

void reboot(){
FILE *inval;

int value;
inval = fopen("/sys/class/gpio/gpio46/value", "r");
fseek(inval,0,SEEK_SET);
fscanf(inval,"%d",&value);
fclose(inval);
//printf("value: %d\n",value);

if (value == 1){
system("reboot");
}
//else printf("Im still running");

}

void Push2Start(){

FILE *inval;
int value;

do {
inval = fopen("/sys/class/gpio/gpio47/value", "r");
fseek(inval,0,SEEK_SET);
fscanf(inval,"%d",&value);
fclose(inval);
sleep(1);
//printf("value: %d\n",value);
} while(value != 1);
}

///////////////////////////////////////////////////////////////////////////
//////////////  Motor Functions
///////////////////////////////////////////////////////////////////////////

void MotorP(int setvalue){        // Setting period for all motors

char Mclock[10]; /// Making an empty char variable
sprintf(Mclock,"%d", setvalue); /// converting the int to the char

if((MC = fopen(M1Period, "r+")) != NULL){ //Opening the file path
 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 1 period
 fclose(MC); //Closing file
}

if((MC = fopen(M2Period, "r+")) != NULL){ //Opening the file path
 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 2 period
 fclose(MC); //Closing file
}

if((MC = fopen(M3Period, "r+")) != NULL){ //Opening the file path
 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 3 period
 fclose(MC); //Closing file
}
}

void MotorLL(int speed){ // Left Motor M1         Setting Duty cycle

char duty[10];
sprintf(duty, "%d", speed);

if((MC = fopen(M1Duty, "r+")) != NULL){ //Opening the file path
fwrite(duty, sizeof(unsigned long int), 4, MC); //Setting Motor 1 duty
fclose(MC); //Closing file
}
}


void MotorRR(int speed){ // Right Motor M2        Setting Duty cycle

char duty[10];
sprintf(duty, "%d", speed);

if((MC = fopen(M2Duty, "r+")) != NULL){ //Opening the file path
fwrite(duty, sizeof(unsigned long int), 4, MC); //Setting Motor 2 duty
fclose(MC); //Closing file
}
}

/////////////////////////////////////////////////////////////////////////////////
////////////////// GPIO pin Functions
/////////////////////////////////////////////////////////////////////////////////

void GPIO_46(){

FILE *in,*indir;

in = fopen("/sys/class/gpio/export", "w"); // Exporting GPIO 46 (P8_16)
fseek(in,0,SEEK_SET);
fprintf(in,"%d",46);
fflush(in);

indir = fopen("/sys/class/gpio/gpio46/direction", "w"); // Setting Pin 46 Direction as input
fseek(indir,0,SEEK_SET);
fprintf(indir,"in");
fflush(indir);

fclose(in); //Closing file
fclose(indir); //Closing file

}

void GPIO_47(){

FILE *in,*indir;

in = fopen("/sys/class/gpio/export", "w"); // Exporting GPIO 47 (P8_15)
fseek(in,0,SEEK_SET);
fprintf(in,"%d",47);
fflush(in);

indir = fopen("/sys/class/gpio/gpio47/direction", "w"); // Setting Pin 47 Direction as input
fseek(indir,0,SEEK_SET);
fprintf(indir,"in");
fflush(indir);

fclose(in); //Closing file
fclose(indir); //Closing file

}



void ADC_setup(){ // Setting up ADC for BBB
FILE *ain;
ain = fopen("/sys/devices/bone_capemgr.8/slots", "w");
fseek(ain,0,SEEK_SET);
fprintf(ain,"cape-bone-iio");
fflush(ain);

fclose(ain);
}

///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////  Robot routines
////////////////////////////////////////////////////////////////////////////////////////

void Stop(){ // This one is Set
MotorLL(18490000);
MotorRR(18500000);
}

void GoStraight(){ // This one is Set
MotorLL(17600000);
MotorRR(18680000);
}

void TurnRight(){ // This one is Set
MotorLL(17600000);
MotorRR(18300000);
usleep(750000);
Stop();
sleep(1);
}

void ShortRight(){
MotorLL(17600000);
MotorRR(18300000);
usleep(350000);
Stop();
sleep(1);
}

void TurnLeft(){ // This one is Set
MotorLL(18700000);
MotorRR(19800000);
usleep(750000);
Stop();
sleep(1);
}

void ShortLeft(){
MotorLL(18700000);
MotorRR(19800000);
usleep(350000);
Stop();
sleep(1);
}

void Look_Left(){
MotorEyes(17300000);
}

void Look_ShortLeft(){
MotorEyes(17800000);
}

void Look_TinyLeft(){
MotorEyes(18150000);
}

void Look_Ahead(){
MotorEyes(18350000);
}

void Look_Right(){
MotorEyes(19300000);
}

void Look_ShortRight(){
MotorEyes(18900000);
}

void Look_TinyRight(){
MotorEyes(18500000);
}

/////////////////////////////////////////////////////////////////////////////////////////
////////////////// IR Distance Sensor
/////////////////////////////////////////////////////////////////////////////////////////

void MotorEyes(int direction){
char duty[10];
sprintf(duty, "%d", direction);

if((MC = fopen(M3Duty, "r+")) != NULL){    //Opening the file path
fwrite(duty, sizeof(unsigned long int), 4, MC);    //Setting Motor 3 position
fclose(MC); //Closing file
}
}

int distance(){

int num = 0;
int value,avg;
for ( int n=20; n>0; n-- ){  // Reads the sensor 20 times

FILE *aval;
aval = fopen("/sys/devices/ocp.2/helper.14/AIN5", "r");
fseek(aval,0,SEEK_SET);
fscanf(aval,"%d",&value); // Reading AIN5
fclose(aval);
usleep(10);

num += value;

if (n == 1){
avg = num/20; // Gets the average distance read
}

}
return avg;
}

/////////////////////////////////////////////////////////////////////
/////////// find lowest value function
////////////////////////////////////////////////////////////////////
int Turn(int W,int NW,int NE,int E){
int of_W_NW = W < NW ? W : NW;
int of_NE_E = NE < E ? NE : E;
return of_W_NW < of_NE_E ? of_W_NW : of_NE_E;

}

The CODE (REV 1)

With this code the robot avoids obstacles fairly well. I am going to add more to make it better. The video on this page is running this same exact code.

//=======================================================================

// Name        : Fianl_Rev1.cpp

// Author      : George

// Version     :

// Copyright   : Your copyright notice

// Description : Revision 1 Robot avoidance program, Ansi-style

//=======================================================================

#include <stdio.h>

#include <stdlib.h>

#include <unistd.h>

using namespace std;

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

/////////       Motor File Paths "can be put in header file"

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

FILE *MC = NULL;

char *M1Period = "/sys/devices/ocp.2/pwm_test_P8_13.10/period"; //P8_13 Left Motor

char *M1Duty = "/sys/devices/ocp.2/pwm_test_P8_13.10/duty";     //P8_13 Left Motor

char *M2Period = "/sys/devices/ocp.2/pwm_test_P8_34.11/period"; //P8_34 Right Motor

char *M2Duty = "/sys/devices/ocp.2/pwm_test_P8_34.11/duty";     //P8_34 Right Motor

char *M3Period = "/sys/devices/ocp.2/pwm_test_P9_21.12/period"; //P9_21 Micro Servo

char *M3Duty = "/sys/devices/ocp.2/pwm_test_P9_21.12/duty";     //P9_21 Micro Servo

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

/////////       Declared Functions  "can be put in header file"

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

void GPIO_46();

void GPIO_47();

void ADC_setup();

int distance();

void setup();

void MotorP(int);

void MotorLL(int);

void MotorRR(int);

void MotorEyes(int);

void Look_Right();

void Look_Left();

void Look_Ahead();

void reboot();

void Push2Start();

void GoStraight();

void TurnRight();

void TurnLeft();

void Stop();

int main(int argc, char** argv)

{

int east,west,north,check_dist;

setup();      // Setting pins, modes, and motors

printf(" You can Push De Button!! \n");

Push2Start(); // Waiting until button is pressed to proceed

while(1){

GoStraight();

check_dist = distance();

printf("  distance = %d  \n", check_dist);

if ( check_dist >= 1100){

Stop();

for (int ii=1; ii<4; ii++){

switch(ii){

case 1:

Look_Left();

usleep(400000);

west=distance();

break;

case 2:

Look_Ahead();

usleep(400000);

north=distance();

break;

case 3:

Look_Right();

usleep(400000);

east=distance();

Look_Ahead();

break;

}

}

printf("  West  = %d  \n", west);

printf("  North = %d  \n", north);

printf("  East  = %d  \n", east);

if (west > east){

TurnRight();

}

else TurnLeft();

}

reboot(); // Checking if reset button is pressed

}

return 0;

}

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

////////////// Setup Function

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

void setup(){

GPIO_46();         // Exporting and setting PinMode (P8_16)

usleep(20);

GPIO_47();   // Exporting and setting PinMode (P8_15)

usleep(20);

ADC_setup();   // Turning Analog pin 5 on

usleep(20);

MotorP(20000000);  // Setting period for all motors

usleep(20);

Stop();   // Motors in stopped position

usleep(20);

Look_Ahead();      // IR Looking forward

usleep(20);

}

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

///////////// Button Functions

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

void reboot(){

FILE *inval;

int value;

inval = fopen("/sys/class/gpio/gpio46/value", "r");

fseek(inval,0,SEEK_SET);

fscanf(inval,"%d",&value);

fclose(inval);

//printf("value: %d\n",value);

if (value == 1){

system("reboot");

}

//else printf("Im still running");

}

void Push2Start(){

FILE *inval;

int value;

do {

inval = fopen("/sys/class/gpio/gpio47/value", "r");

fseek(inval,0,SEEK_SET);

fscanf(inval,"%d",&value);

fclose(inval);

sleep(1);

//printf("value: %d\n",value);

} while(value != 1);

}

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

//////////////  Motor Functions

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

void MotorP(int setvalue){        // Setting period for all motors

char Mclock[10]; /// Making an empty char variable

sprintf(Mclock,"%d", setvalue); /// converting the int to the char

if((MC = fopen(M1Period, "r+")) != NULL){ //Opening the file path

 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 1 period

 fclose(MC); //Closing file

}

if((MC = fopen(M2Period, "r+")) != NULL){ //Opening the file path

 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 2 period

 fclose(MC); //Closing file

}

if((MC = fopen(M3Period, "r+")) != NULL){ //Opening the file path

 fwrite(Mclock, sizeof(unsigned long int), 4, MC); //Setting Motor 3 period

 fclose(MC); //Closing file

}

}

void MotorLL(int speed){ // Left Motor M1         Setting Duty cycle

char duty[10];

sprintf(duty, "%d", speed);

if((MC = fopen(M1Duty, "r+")) != NULL){ //Opening the file path

fwrite(duty, sizeof(unsigned long int), 4, MC); //Setting Motor 1 period

fclose(MC); //Closing file

}

}

void MotorRR(int speed){ // Right Motor M2        Setting Duty cycle

char duty[10];

sprintf(duty, "%d", speed);

if((MC = fopen(M2Duty, "r+")) != NULL){ //Opening the file path

fwrite(duty, sizeof(unsigned long int), 4, MC); //Setting Motor 1 period

fclose(MC); //Closing file

}

}

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

////////////////// GPIO pin Functions

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

void GPIO_46(){

FILE *in,*indir;

in = fopen("/sys/class/gpio/export", "w"); // Exporting GPIO 46 (P8_16)

fseek(in,0,SEEK_SET);

fprintf(in,"%d",46);

fflush(in);

indir = fopen("/sys/class/gpio/gpio46/direction", "w"); // Setting Pin 46 Direction as input

fseek(indir,0,SEEK_SET);

fprintf(indir,"in");

fflush(indir);

fclose(in); //Closing file

fclose(indir); //Closing file

}

void GPIO_47(){

FILE *in,*indir;

in = fopen("/sys/class/gpio/export", "w"); // Exporting GPIO 47 (P8_15)

fseek(in,0,SEEK_SET);

fprintf(in,"%d",47);

fflush(in);

indir = fopen("/sys/class/gpio/gpio47/direction", "w"); // Setting Pin 47 Direction as input

fseek(indir,0,SEEK_SET);

fprintf(indir,"in");

fflush(indir);

fclose(in); //Closing file

fclose(indir); //Closing file

}

void ADC_setup(){ // Setting up ADC for BBB

FILE *ain;

ain = fopen("/sys/devices/bone_capemgr.8/slots", "w");

fseek(ain,0,SEEK_SET);

fprintf(ain,"cape-bone-iio");

fflush(ain);

fclose(ain);

}

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

/////////////////  Robot routines

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

void Stop(){ // This one is Set

MotorLL(18490000);

MotorRR(18500000);

}

void GoStraight(){ // This one is Set

MotorLL(17600000);

MotorRR(18600000);

}

void TurnRight(){ // This one is Set

MotorLL(17600000);

MotorRR(18300000);

usleep(750000);

Stop();

sleep(1);

}

void TurnLeft(){ // This one is Set

MotorLL(18700000);

MotorRR(19800000);

usleep(700000);

Stop();

sleep(1);

}

void Look_Left(){

MotorEyes(17300000);

}

void Look_Ahead(){

MotorEyes(18350000);

}

void Look_Right(){

MotorEyes(19300000);

}

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

////////////////// IR Distance Sensor

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

void MotorEyes(int direction){

char duty[10];

sprintf(duty, "%d", direction);

if((MC = fopen(M3Duty, "r+")) != NULL){    //Opening the file path

fwrite(duty, sizeof(unsigned long int), 4, MC);    //Setting Motor 3 position

fclose(MC); //Closing file

}

}

int distance(){

int num = 0;

int value,avg;

for ( int n=20; n>0; n-- ){  // Reads the sensor 20 times

FILE *aval;

aval = fopen("/sys/devices/ocp.2/helper.14/AIN5", "r");

fseek(aval,0,SEEK_SET);

fscanf(aval,"%d",&value); // Reading AIN5

fclose(aval);

usleep(10);

num += value;

if (n == 1){

avg = num/20; // Gets the average distance read

}

}

return avg;

}

The Build (Robot)

To build the frame of the robot I used hobby plates and brackets that I got from Lowe's. Figure 0 shows three of the style parts used to build the frame. There were various designs that I came up with but the best one is the one that can be seen in Figure 1.

Figure 0; Parts used for frame

• 1 3x3 plate
• 2 8x8 plate
• 4 1x6 brace
• 1 L-bracket
• 4 half inch spacers
• standard hardware

Figure 1: Final Frame Design

With this design the robot is very easy to break down. The two platforms that hold the BeagleBone Black and the Anker battery are free floating. When I remove the battery I can easily remove the wheels and servos. I did this so I could do a quick field dis-assembly in case I needed the servos for another project.


The platform that holds the BeagleBone Black is a perfect size for holding a mini-breadboard. I will need the breadboard to add components such as resistors and buttons. It also will make connecting the servos and the IR distance sensor a lot easier. See Figure 2.

Figure 2: Mini-Breadboard

To mount the micro-servo on the front of the robot I used a hot glue gun. I also used hot glue to hold the BeagleBone Black case. With everything solid I was then able to wire the various components.

For the IR distance sensor I had to make a voltage divider circuit because the analog pins on the BBB can only handle up to 1.8 V. To make sure I was not going to brick my BBB I connected the IR sensor to a multimeter to check the voltage divider circuit in Figure 3. The voltage divider never went over 1.6 V on the multimeter so it should work.

Figure 3; Voltage divider

With everything ready I wired up the servos, IR sensor, and two push buttons. It looks like a mess but everything connected to the BBB can be seen in Figure 4 and Figure 5.

Figure 4; Everything wired and mounted

Figure 5; Wired Breadboard