//=======================================================================
// 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;
}