int main (int argc, char *argv[]) { // so that redirected stdout won't be insanely buffered. setvbuf (stdout, (char *) NULL, _IONBF, 0); lcm_t *lcm = lcm_create (NULL); if(!lcm) return 1; printf ("utime,\t\tleft_ticks,\tright_ticks\n"); maebot_motor_feedback_t_subscribe (lcm, "MAEBOT_MOTOR_FEEDBACK", motor_feedback_handler, NULL); while (1) lcm_handle (lcm); return 0; }
int main (int argc, char *argv[]) { // so that redirected stdout won't be insanely buffered. setvbuf (stdout, (char *) NULL, _IONBF, 0); maebot_shared_state_t sharedState; initState(&sharedState); sharedState.running = 1; lcm_t *lcm = lcm_create(NULL); if(!lcm) return EXIT_FAILURE; maebot_sensor_data_t_subscribe(lcm, "MAEBOT_SENSOR_DATA", sensor_data_handler, &sharedState); maebot_processed_sensor_data_t_subscribe(lcm, "MAEBOT_PROCESSED_SENSOR_DATA", processed_sensor_data_handler, &sharedState); maebot_motor_feedback_t_subscribe(lcm, "MAEBOT_MOTOR_FEEDBACK", motor_feedback_handler, &sharedState); pose_xyt_t_subscribe(lcm, "BOTLAB_ODOMETRY", pose_xyt_handler, &sharedState); pthread_t lcm_thread; pthread_create(&lcm_thread, NULL, lcm_handler, lcm); // Wait for first sensor data to come in //printf("Waiting for a sensor datum to come in ... "); waitForSensorUpdate(&sharedState.sData.utime_sama5, &sensor_data_mutex); //printf("Done\n"); //printf("Waiting for a odometry datum to come in ... "); waitForSensorUpdate(&sharedState.oData.utime, &sensor_data_mutex); //printf("Done\n"); // printf("Waiting for a processed sensor datum to come in ... "); waitForSensorUpdate(&sharedState.pData.utime_sama5, &sensor_data_mutex); //printf("Done\n"); pthread_mutex_lock( &sensor_data_mutex); for(int i=0; i<3; i++) { sharedState.startup_int[i] = sharedState.sData.gyro_int[i]; } pthread_mutex_unlock(&sensor_data_mutex); //pthread_t print_thread; //pthread_create(&print_thread, NULL, print_handler, &sharedState); pthread_t plot_thread; // data for plots and graphs pthread_create(&plot_thread, NULL, plot_handler, &sharedState); for(;;){;} // will probably never get here, // but if for some reason I do, close gracefully sharedState.running = 0; usleep(10000); // a little time to let buffers clear return EXIT_SUCCESS; }
int main (int argc, char *argv[]) { // so that redirected stdout won't be insanely buffered. setvbuf (stdout, (char *) NULL, _IONBF, 0); state_t *state = calloc (1, sizeof *state); state->meters_per_tick = METERS_PER_TICK; // IMPLEMENT ME state->alpha = ALPHA; state->beta = BETA; // file for gyro integration test //state->fp = fopen("gyro-yaw-integration.txt","w"); state->fp = fopen("yaw.txt","w"); // Tests for the position estimate /* update_position(state,4456,4456); update_position(state,0,1208); update_position(state,4456,4456); while(1); */ // Used for testing the gyro bias /* for(int i=0; i<100; i++){ state->yaw_cal_array[i] = 15*i; } state->gyro_bias = find_gyro_bias(state); printf("Gyro Bias: %llu\n", state->gyro_bias); */ //printf("getting options\n"); state->gopt = getopt_create (); getopt_add_bool (state->gopt, 'h', "help", 0, "Show help"); getopt_add_bool (state->gopt, 'g', "use-gyro", 0, "Use gyro for heading instead of wheel encoders"); getopt_add_string (state->gopt, '\0', "odometry-channel", "BOTLAB_ODOMETRY", "LCM channel name"); getopt_add_string (state->gopt, '\0', "feedback-channel", "MAEBOT_MOTOR_FEEDBACK", "LCM channel name"); getopt_add_string (state->gopt, '\0', "sensor-channel", "MAEBOT_SENSOR_DATA", "LCM channel name"); getopt_add_double (state->gopt, '\0', "alpha", ALPHA_STRING, "Longitudinal covariance scaling factor"); getopt_add_double (state->gopt, '\0', "beta", BETA_STRING, "Lateral side-slip covariance scaling factor"); getopt_add_double (state->gopt, '\0', "gyro-rms", GYRO_RMS_STRING, "Gyro RMS deg/s"); if (!getopt_parse (state->gopt, argc, argv, 1) || getopt_get_bool (state->gopt, "help")) { printf ("Usage: %s [--url=CAMERAURL] [other options]\n\n", argv[0]); getopt_do_usage (state->gopt); exit (EXIT_FAILURE); } state->use_gyro = getopt_get_bool (state->gopt, "use-gyro"); state->odometry_channel = getopt_get_string (state->gopt, "odometry-channel"); state->feedback_channel = getopt_get_string (state->gopt, "feedback-channel"); state->sensor_channel = getopt_get_string (state->gopt, "sensor-channel"); state->alpha = getopt_get_double (state->gopt, "alpha"); state->beta = getopt_get_double (state->gopt, "beta"); state->gyro_rms = getopt_get_double (state->gopt, "gyro-rms") * DTOR; state->yaw_calibrated = 0; state->yaw = 0; state->yaw_old = 0; //printf("subscribing to channels\n"); // initialize LCM state->lcm = lcm_create (NULL); maebot_motor_feedback_t_subscribe (state->lcm, state->feedback_channel, motor_feedback_handler, state); maebot_sensor_data_t_subscribe (state->lcm, state->sensor_channel, sensor_data_handler, state); printf ("ticks per meter: %f\n", 1.0/state->meters_per_tick); while (1){ lcm_handle (state->lcm); } }
int main (int argc, char *argv[]) { // so that redirected stdout won't be insanely buffered. setvbuf (stdout, (char *) NULL, _IONBF, 0); maebot_shared_state_t sharedState; initState(&sharedState); sharedState.running = 1; lcm_t *lcm = lcm_create (NULL); if (!lcm) return EXIT_FAILURE; maebot_sensor_data_t_subscribe(lcm, "MAEBOT_SENSOR_DATA", sensor_data_handler, &sharedState); maebot_motor_feedback_t_subscribe(lcm, "MAEBOT_MOTOR_FEEDBACK", motor_feedback_handler, &sharedState); pthread_t lcm_thread; pthread_create(&lcm_thread, NULL, lcm_handler, lcm); int64_t bias[3]; int64_t startTime = 0; int64_t stopTime = 0; int64_t deltaTime = 0; int64_t biasArr[10][3]; int64_t utime = 0; // clear array for(int i=0; i<10; i++) for(int j=0; j<3; j++) biasArr[i][j] = 0; // Wait for first sensor data to come in pthread_mutex_lock( &sensor_data_mutex); utime = sharedState.sensorData.utime_sama5; pthread_mutex_unlock(&sensor_data_mutex); while(utime == 0) { usleepClock(100000, &sharedState.sensorData.utime_sama5, &sensor_data_mutex); pthread_mutex_lock( &sensor_data_mutex); utime = sharedState.sensorData.utime_sama5; pthread_mutex_unlock(&sensor_data_mutex); } pthread_mutex_lock( &sensor_data_mutex); for(int i=0; i<3; i++) { sharedState.startup_int[i] = sharedState.sensorData.gyro_int[i]; } sharedState.startup_time = sharedState.sensorData.utime_sama5; pthread_mutex_unlock(&sensor_data_mutex); //pthread_t print_thread; //pthread_create(&print_thread, NULL, print_handler, &sharedState); //pthread_t plot_thread; // data for plots and graphs //pthread_create(&plot_thread, NULL, plot_handler, &sharedState); for(;;) { // Get start state for static calibration pthread_mutex_lock( &sensor_data_mutex); maebot_sensor_data_t startSensorState = sharedState.sensorData; maebot_motor_feedback_t startMotorFeedback = sharedState.motorFeedback; pthread_mutex_unlock(&sensor_data_mutex); usleepClock(500000, &sharedState.sensorData.utime_sama5, &sensor_data_mutex); // Get end state for static calibration pthread_mutex_lock( &sensor_data_mutex); maebot_sensor_data_t stopSensorState = sharedState.sensorData; maebot_motor_feedback_t stopMotorFeedback = sharedState.motorFeedback; utime = sharedState.sensorData.utime_sama5; pthread_mutex_unlock(&sensor_data_mutex); startTime = startSensorState.utime_sama5; stopTime = stopSensorState.utime_sama5; deltaTime = stopTime - startTime; if(deltaTime < 1000) { // do not accept chunks of time less than 1ms continue; // Time roll over: ignore } if(stopMotorFeedback.encoder_left_ticks != startMotorFeedback.encoder_left_ticks) { continue; } if(stopMotorFeedback.encoder_right_ticks != startMotorFeedback.encoder_right_ticks) { continue; } //printf("Updating calibration Array\n"); // If I get here, then the calibration succeded. double ddeltaTime = (double)deltaTime/1000000.0; // conv to seconds for(int i = 0; i<3; i++) { int64_t startInt = startSensorState.gyro_int[i]; int64_t stopInt = stopSensorState.gyro_int[i]; bias[i] = (int64_t)(((double)(stopInt - startInt) / ddeltaTime )); //bias[i] = stopInt - startInt; } //bias[0] = 1000000000; //bias[1] = -1000000000; //bias[2] = INT16_MAX+1; /* printf("\n"); printf("start time = % lld\n", startSensorState.utime_sama5); printf("stop time = % lld\n", stopSensorState.utime_sama5); printf("delta time = % lld\n", deltaTime); for(int i = 0; i<3; i++) printf("bias[%2d] = % d\n", i, bias[i]); */ // shift the data down array // its only 27 elements so dynamic allocaton seems unessisary int64_t sum[3] = {0, 0, 0}; for(int i=10-1; i>0; i--) { for(int j=0; j<3; j++) { sum[j] += biasArr[i][j] = biasArr[i-1][j]; } } for(int j=0; j<3; j++) { sum[j] += biasArr[0][j] = bias[j]; bias[j] = sum[j]/10; } pthread_mutex_lock( &sensor_data_mutex); for(int j=0; j<3; j++) { sharedState.processedSensorData.gyroBias[j] = bias[j]; sharedState.gyroBias[j] = bias[j]; } sharedState.valid++; pthread_mutex_unlock(&sensor_data_mutex); //printf("bias = % lld\n", bais[0]); if(sharedState.valid > 10) { for(;;) sleep(1); } } // will probably never get here, // but if for some reason I do, close gracefully sharedState.running = 0; //pthread_join(print_thread, NULL); usleep(10000); // a little time to let buffers clear return EXIT_SUCCESS; }