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;
}
No comments:
Post a Comment