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 EXIT_FAILURE;
maebot_sensor_data_t_subscribe (lcm,
"MAEBOT_SENSOR_DATA",
sensor_data_handler,
NULL);
while (1)
lcm_handle (lcm);
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_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);
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;
}
