#include #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) double gaussDist(void); 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 index of the sonar which detects the closest object and is less than min_dist */ short shortest_front_sonar(long *state, int min_dist, int noise) { int i, shortest, shortest_sonar; shortest = 255; shortest_sonar = 0; i = 17; do { printf("%ld ", state[i]); if (state[i] < shortest) { shortest = (short)state[i]; shortest_sonar = i-16; } i++; if (i == 20) i = 31; } while (i < 33); if (shortest > min_dist) shortest_sonar = 0; printf("shortest: %d\n", shortest_sonar); // add some Gaussian noise shortest_sonar += gaussDist() * noise; 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 noise = 20; // inches, big noise 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; struct timeb tb; float x; // state estimate is distance from the wall, in inches float xminus; float xplus; float z; // measurement is distance from the wall, in inches float K; // Kalman gain float P; // covariance matrix float phi; // motion model = dT*v float H; // H = [1] float R; // measurement noise float Q; // system noise (small) // Note how we calculate time so that we don't get roundoff error in floats time_t basetime; time_t difftime; double lasttime; double curtime; FILE *fp; /* set up */ if(argc > 1) { desiredDistance = atof(argv[1]); } printf("Maintaining distance: %.2f\n", desiredDistance); if(argc > 2) { noise = atof(argv[2]); } printf("Using noise value: %.2f\n", noise); R = noise; // R initialized here if given on command line if(argc > 3) { R = atof(argv[3]); } printf("Using Noise model: %.2f\n", R); /* 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); // initialize the state based on a single sonar reading gs(); // initial state x = State[STATE_SONAR_0]; // initial probability P = 1.0; // TODO: initialize phi, Q, and H // NOTE: if you use a 2-element state vector, phi will change each time // H = // Q = // phi = // prior velocity reading ix = 0; // initialize last time ftime(&tb); basetime = tb.time; lasttime = (double)(tb.millitm) / 1000.0; fp = fopen("data", "w"); /* the loop */ while(1) { gs(); // calculate distance from the wall, measurement currentDistance = (float)State[STATE_SONAR_0] + gaussDist() * noise; // put measurement into z variable, this is z_t z = currentDistance; // calculate the current time ftime(&tb); difftime = tb.time - basetime; curtime = (double)(difftime) + (double)(tb.millitm) / 1000.0; // TODO, fill in the Kalman filter steps and base the control system on the Kalman state // predictive step, calculate xminus // uncertainty increase due to open loop prediction, modify P // Calculate Kalman filter gain, K // uncertainty decrease due to measurement, modify P // state update due to measurement, calculate xplus // update the current state using xplus x = xplus; // proportional control law section // calculate error using state estimate error = desiredDistance - x; velocity = kp * error; printf("K %.2f P %.2f dt %.4f\n", K, P, curtime - lasttime); printf("d %.2f s %.2f x %.2f E %.2f\n", desiredDistance, currentDistance, x, error); // remember to pass on current time lasttime = curtime; if(error < errorThreshold && error > -errorThreshold) { ix = 0; } else { // 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; } fprintf(fp, "%f %.2f %.2f %d\n", curtime, currentDistance, x, ix); // call scout_vm scout_vm(ix, 0); usleep(100000); } fclose(fp); conf_sn(15, order); scout_vm(0, 0); disconnect_robot(1); return(0); }