#include #include #include #include "Nclient.h" #define ROTATION_CONSTANT 0.118597 /* inches/degree (known to 100 ppm) */ #define RIGHT(trans, steer) (trans + (int)((float)steer*ROTATION_CONSTANT)) #define LEFT(trans, steer) (trans - (int)((float)steer*ROTATION_CONSTANT)) #define scout_vm(trans, steer) vm(RIGHT(trans, steer), LEFT(trans, steer), 0) #define scout_pr(trans, steer) pr(RIGHT(trans, steer), LEFT(trans, steer), 0) static void lclsighandler ( int signal ) { int order[16] = {255}; fprintf(stderr, "Caught signal %d, exiting.\n", signal ); // for all signals, detach from the robot and turn off the sonars conf_sn(15, order); scout_vm(0, 0); disconnect_robot(1); exit ( -1 ); } int setupSigHandler(void (*sigh)(int)) { if(sigh == NULL) sigh = lclsighandler; // setup the signal handler signal ( SIGINT, sigh ); signal ( SIGQUIT, sigh ); signal ( SIGTERM, sigh ); signal ( SIGHUP, sigh ); signal ( SIGFPE, sigh ); return(0); } void turn_front_sonar_on(void) { int sn_order[16] = {0, 2, 15, 1, 14, 3, 13, 4, 12, 5, 11, 6, 10, 7, 9, 8}; /* Remainder are irrelevant */ /* turn on 5 front sonars and set them to fire in certain order */ conf_sn(15, sn_order); conf_tm(5); } /* monitor the active sonar as specified in parameter "order" */ /* returns the sonar which detects the closest object and is less than min_dist */ short shortest_front_sonar(long *state, short min_dist) { short i, shortest, shortest_sonar; shortest = 255; shortest_sonar = 0; i = 17; do { if (state[i] < shortest) { shortest = (short)state[i]; shortest_sonar = i-16; } printf("%d: %d ", i-17, state[i]); i++; if (i == 20) i = 31; } while (i < 33); if (shortest > min_dist) shortest_sonar = 0; printf("\n"); return(shortest_sonar); } /* * main */ int main(int argc, char *argv[]) { short c; int order[16] = {255}; float desiredDistance = 1.0 * 100 * (1.0 / 2.54); // one meter, in inches float currentDistance = 0.0; float error = 0.0; float kp = -5.0; float velocity = 0.0; float errorThreshold = 10.0 / 2.54; // 10 cm, in inches int ix = 0; int maxVelocity = 200; int minVelocity = 10; /* set up */ /* connect to the robot */ connect_robot(1, MODEL_SCOUT2, "/dev/ttyS0", 38400); /* turn on the front sonars */ turn_front_sonar_on(); /* setup the signal handler */ setupSigHandler(NULL); /* the loop */ while(1) { gs(); // calculate distance from the wall currentDistance = (float)State[STATE_SONAR_0]; // calculate error error = desiredDistance - currentDistance; velocity = kp * error; printf("d %.2f s %.2f E %.2f\n", desiredDistance, currentDistance, error); if(error < errorThreshold && error > -errorThreshold) { scout_vm(0, 0); continue; } // convert to an integer and bound ix = (int)velocity; ix = ix > maxVelocity ? maxVelocity : ix; ix = ix < - maxVelocity ? -maxVelocity : ix; ix = ix < minVelocity && ix > 0 ? minVelocity : ix; ix = ix > -minVelocity && ix < 0 ? -minVelocity : ix; // call scout_vm scout_vm(ix, 0); usleep(10000); } conf_sn(15, order); scout_vm(0, 0); disconnect_robot(1); return(0); }