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;

}