int main(int argc, char *argv[]){
//The gpio port numbers needed to drive the motor
unsigned int gpio1 = 30;
unsigned int gpio2 = 31;
unsigned int gpio3 = 48;
unsigned int gpio4 = 51;
unsigned int gpio5 = 15;
unsigned int gpios[4] = {gpio1, gpio2, gpio3, gpio4};
int gpio_fd1, gpio_fd2, gpio_fd3, gpio_fd4, gpio_fd5;
int behavior = atoi(argv[1]);
int rc;
int len;
int nfds = 2;
//Port numbers for the analog in
char AnalogIn1[] = "AIN0";
char AnalogIn2[] = "AIN1";
//char analogs[] = {AnalogIn1, AnalogIn2};
//printf("checking init analogs: %s \n", analogs[0]);
char buf[64];
//Arrays to hold the values we get back from the sensors
struct pollfd fdset[2];
int Sensor1_val;
int Sensor2_val;
//int Sensor_sum;
int mode = atoi(argv[1]);
//Initializes the position as zero, but we dont really know where this is
int pos = 0;
//Set up the gpios so we can use them to drive the motor
gpio_export(gpio1);
gpio_export(gpio2);
gpio_export(gpio3);
gpio_export(gpio4);
gpio_export(gpio5);
gpio_set_dir(gpio1, "out");
gpio_set_dir(gpio2, "out");
gpio_set_dir(gpio3, "out");
gpio_set_dir(gpio4, "out");
gpio_set_dir(gpio5, "in");
gpio_set_edge(gpio5, "rising");
gpio_fd1 = gpio_fd_open(gpio1, O_RDONLY);
gpio_fd2 = gpio_fd_open(gpio2, O_RDONLY);
gpio_fd3 = gpio_fd_open(gpio3, O_RDONLY);
gpio_fd4 = gpio_fd_open(gpio4, O_RDONLY);
gpio_fd5 = gpio_fd_open(gpio5, O_RDONLY);
int cycle = 0;
int minValue = 999999;
int minPosition = 0;
int Sensor_avg = 0;
//Wait for the start button to be pushed before we acutally start doing anything
printf("Press the start button to begin the program \n");
len = read(gpio_fd5, buf, 64);
while(1){
memset((void*)fdset, 0, sizeof(fdset));
fdset[0].fd = STDIN_FILENO;
fdset[0].events = POLLIN;
fdset[1].fd = gpio_fd5;
fdset[1].events = POLLPRI;
rc = poll(fdset, nfds, 3000);
if(rc < 0){
printf("Poll failed \n");
}
if(fdset[1].revents & POLLPRI){
lseek(fdset[1].fd, 0, SEEK_SET);
len = read(fdset[1].fd, buf, 64);
printf("Triggered \n");
//pos = problemOne(gpios, pos);
pos = problemTwo(gpios, AnalogIn1, AnalogIn2, pos, mode);
//break;
}
}
printf("Begining our steps to attempt to come up with ");
for(cycle = 0; cycle <= MAX_CYCLES; cycle++){
printf("One cycle number: %d", cycle);
Sensor1_val = analogIn(AnalogIn1);
Sensor2_val = analogIn(AnalogIn2);
Sensor_avg = (Sensor1_val + Sensor2_val)/ 2;
if(Sensor_avg < minValue){
minValue = Sensor_avg;
minPosition = cycle;
}
//Should move the IR sensors here
}
printf("%d\n", minValue);
if(minPosition <=10){
//Rotate clockwise to position
}
else{
//Rotate counterclockwise to position
}
while(1){
Sensor1_val = analogIn(AnalogIn1);
Sensor2_val = analogIn(AnalogIn2);
if(Sensor1_val > Sensor2_val){
//Rotate clockwise once
}
else{
//Rotate counterclockwise
}
}
return 0;
}
/****************************************************************
* Main
****************************************************************/
int main(int argc, char *argv[]){
char PT1[] = "AIN4", PT2[] = "AIN6";
int PT1_val[20], PT2_val[20], PT_sum[20];
int min, minPos, pos, PT1_now, PT2_now, i;
// Set the signal callback for Ctrl-C
signal(SIGINT, signal_handler);
initIO();
pos = 0;
//Since we don't know the initial position of motor, we'd better let it rotate 2 steps first
cRotate(&pos);
cRotate(&pos);
//Clockwise rotate for a cycle and record the value in different directions
for(i = 0; i < 20; i++) {
PT1_val[i] = analogIn(PT1);
PT2_val[i] = analogIn(PT2);
printf("PT1:%4d PT2:%4d\n", PT1_val[i], PT2_val[i]);
cRotate(&pos);
delay(100);
}
for(i = 0; i < 20; i++) {
PT_sum[i] = PT1_val[i] + PT2_val[i];
}
min = 50000; //Initialize a large number as min to garantee to be replaced later
//Find the direction with minimum value, which has the strongest light
for(i = 0; i < 20; i++)
if(PT_sum[i] < min) {
min = PT_sum[i];
minPos = i;
}
printf("min:%d minPos:%d\n", min, minPos);
//Counter clockwise rotate to the direction with strongest light
for(i = 19; i >= 0; i--) {
ccRotate(&pos);
delay(100);
if((i-minPos) == 0) break;
}
//Tracking mode. Find the strongest light and rotate to its direction
while(keepgoing) {
//Read analog inputs
PT1_now = analogIn(PT1);
PT2_now = analogIn(PT2);
//Set a threshold. If the difference between the phototransistors is too small, we will not rotate
if (PT1_now - PT2_now < 150 && PT1_now - PT2_now > -150){
delay(300);
}
//Decide which direction to rotate
else if (PT1_now > PT2_now){
cRotate(&pos);
}
else if (PT1_now < PT2_now){
ccRotate(&pos);
}
printf("PT1:%4d PT2:%4d\n", PT1_now, PT2_now);
delay(500);
}
return 0;
}
//MAIN function.
int main(int argc, char *argv[]){
controlGPIOs[0] = 23;
controlGPIOs[1] = 47;
controlGPIOs[2] = 26;
controlGPIOs[3] = 46;
//setup Analog IO's
system("./analogInSetup.sh");
int i, k, samples[20], min, toggle, minIndex, contine, counter, minFound;
minFound = 0;
min = 99999;
counter = 0;
contine = 1;
if(argc < 3){
printf("Fewers than 2 arguments detected: using default values.\n");
ain[0] = '2';
ain[1] = '1';
controlGPIOs[4] = 65;
}else{
ain[0] = argv[2][0];
ain[1] = argv[3][0];
controlGPIOs[4] = atoi(argv[1]);
}
//Initialize IO
initIO();
while(contine){
gpio_get_value(controlGPIOs[4], &toggle);
//if toggle pressed, rotate once then counter-rotate to light source
if(toggle == 0){
//Clockwise rotate for a cycle and record the value in different directions
for(i=0; i<5; i++){
for(k=0; k<4; k++){
rotateClock(k);
samples[counter] = analogIn(ain[0])+analogIn(ain[1]);
if(min>samples[counter]){
minIndex=counter;min=samples[counter];}
counter++;
}
}
printf("%d\n", minIndex);
counter = 0;
for(k=0; k<((20-minIndex)/4); k++){
for(i=0; i<4; i++){
rotateClock(3-i);
counter++;
}
}
printf("%d\n", counter);
for(i=0; i<((20-minIndex)%4); i++){
rotateClock(3-i);
counter++;
}
printf("here%d\n", counter);
minFound = 1;
}
//Track light source
if(minFound == 1){
//Moving Left and Right
printf("Ain[0] =%d\tAin[1]=%d\n", analogIn(ain[0]), analogIn(ain[1]) );
if( (analogIn(ain[0]) ) > analogIn(ain[1]) ) {
rotateClock(0);
rotateClock(1);
rotateClock(2);
rotateClock(3);
printf("Should move Clockwise\n");
} else {
rotateClock(3);
rotateClock(2);
rotateClock(1);
rotateClock(0);
printf("Should move Counterclockwise\n");
}
fflush(stdout);
usleep(2000000);
}
}
return 0;
}
