int main( int argn, char **argv ) { int i; // so the program does not complain argn = argn; argv = argv; fprintf( stderr, " started program... \n" ); // initialize carmen_base_direct_initialize_robot( "bot", "/dev/ttyUSB0" ); // set a desired velocity double tv = -0.0; double rv = 0.0; double x, y, theta, tv_out, rv_out; double time; // loop for( i = 0; i <10000; ++i ) { carmen_base_direct_set_velocity( tv, rv ); carmen_base_direct_update_status( &time ); carmen_base_direct_get_integrated_state( &x, &y, &theta, &tv_out, &rv_out ); printf( " \n on iter %d \n ", i ); } // clean up carmen_base_direct_shutdown_robot(); }
static int initialize_robot(void) { int result; result = carmen_base_direct_initialize_robot (model_name, dev_name); if (result < 0) return -1; if (use_sonar) { num_sonar_ranges = carmen_base_direct_sonar_on(); if(num_sonar_ranges < 0) return -1; if (ranges == NULL) free(ranges); ranges = (double *)calloc(num_sonar_ranges, sizeof(double)); carmen_test_alloc(ranges); positions = (carmen_point_t *) calloc(num_sonar_ranges, sizeof(carmen_point_t)); carmen_test_alloc(positions); sonar_state = 1; } else { result = carmen_base_direct_sonar_off(); if(result < 0) return -1; } result = carmen_base_direct_reset(); if (result < 0) return -1; reset_time = carmen_get_time(); odometry.host = carmen_get_host(); return 0; }
int main(int argc, char *argv[]) { struct stat buf; double x, y, theta, tv, rv; double left_disp, right_disp, delta_time; int fd; int unused_return_value; if (argc != 2 || carmen_strncasecmp(argv[1], "-h", 2) == 0 || carmen_strncasecmp(argv[1], "--h", 3) == 0) { if (argc > 2) carmen_warn("Too many arguments.\n\n"); else if (argc < 2) carmen_warn("Not enough arguments.\n\n"); carmen_die("Usage: %s <serial dev name>\n", argv[0]); } // Does the device exist? carmen_warn("Device exists test: "); if (stat(argv[1], &buf) == 0) carmen_warn("OK\n"); else { carmen_warn("FAILED\n"); carmen_die_syserror("Device exists test"); } // Can we open the device? carmen_warn("Device open test: "); fd = open(argv[1], O_RDWR | O_SYNC | O_NOCTTY, 0); if (fd >= 0) carmen_warn("OK\n"); else { carmen_warn("FAILED\n"); carmen_die_syserror("Device open test"); } // close(fd); // Can we talk to the orc board properly? carmen_warn("Orc recognized test: "); if (carmen_base_direct_initialize_robot("orc", argv[1]) == 0) carmen_warn("OK\n"); else { carmen_die("FAILED\n"); } // Can we move the wheels? carmen_warn("Orc drive test: %sMAKE SURE THE ROBOT IS ON BLOCKS%s.\n", carmen_red_code, carmen_normal_code); carmen_warn("Hit return to start the orc drive test...."); unused_return_value = scanf("%*c"); if (1) { // Move left wheel carmen_warn("Left wheel forwards test: "); carmen_base_command_velocity(1, 0, 0); sleep(1); carmen_base_command_velocity(0, 0, 0); sleep(1); carmen_base_query_low_level(&left_disp, &right_disp, &delta_time); // carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time); if (0 && fabs(right_disp) > 1e-3) carmen_die("FAILED\nRight encoder moved. The motors need " "to be swapped.\n"); if (left_disp < -1e-3) carmen_die("FAILED\n Left encoder moved backwards. The motor is " "upside-down.\n"); if (fabs(left_disp) < 1e-3) carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n"); carmen_warn("OK\n"); carmen_warn("Left wheel backwards test: "); carmen_base_command_velocity(-1, 0, 0); sleep(1); carmen_base_command_velocity(0, 0, 0); sleep(1); carmen_base_query_low_level(&left_disp, &right_disp, &delta_time); // carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time); if (0 && fabs(right_disp) > 1e-3) carmen_die("FAILED\nRight encoder moved. Very odd.\n"); if (left_disp > 1e-3) carmen_die("FAILED\n Left encoder moved forwards. Very odd.\n"); if (fabs(left_disp) < 1e-3) carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n"); carmen_warn("OK\n"); // Move right wheel carmen_warn("Right wheel forwards test: "); carmen_base_command_velocity(1, 0, 2); sleep(1); carmen_base_command_velocity(0, 0, 2); sleep(1); carmen_base_query_low_level(&left_disp, &right_disp, &delta_time); // carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time); if (0 && fabs(left_disp) > 1e-3) carmen_die("FAILED\nLeft encoder moved. This is very odd, since it" "also moved with\nthe other wheel.\n"); if (right_disp < -1e-3) carmen_die("FAILED\nRight encoder moved backwards. The motor is " "upside-down.\n"); if (fabs(right_disp) < 1e-3) carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n"); carmen_warn("OK\n"); carmen_warn("Right wheel backwards test: "); carmen_base_command_velocity(-1, 0, 2); sleep(1); carmen_base_command_velocity(0, 0, 2); sleep(1); carmen_base_query_low_level(&left_disp, &right_disp, &delta_time); // carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time); if (0 && fabs(left_disp) > 1e-3) carmen_die("FAILED\nLeft encoder moved. Very odd.\n"); if (right_disp > 1e-3) carmen_die("FAILED\nRight encoder moved forwards. Very odd.\n"); if (fabs(right_disp) < 1e-3) carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n"); } carmen_warn("OK\n"); carmen_base_direct_reset(); carmen_warn("Orc drive test: "); if (carmen_base_direct_set_velocity(1.0, 0.0) == 0) carmen_warn("OK\n"); else carmen_die("FAILED\n"); // Let some encoder data build up carmen_warn("Sleep\n"); sleep(1); carmen_base_direct_set_velocity(0.0, 0.0); // Are we getting encoder data? carmen_warn("Orc encoder test: "); if (carmen_base_direct_get_integrated_state(&x, &y, &theta, &tv, &rv) == 0) carmen_warn("OK\n"); else carmen_die("FAILED\n"); // Is encoder data valid? carmen_warn("Orc encoder data validity test: "); if (fabs(x) > 0 && fabs(y) < 1e-2 && fabs(carmen_radians_to_degrees(theta)) < 10) carmen_warn("OK (%.2f)\n", x); else carmen_die("FAILED (%.2f %.2f %.2f)\n", x, y, carmen_radians_to_degrees(theta)); carmen_base_direct_shutdown_robot(); return 0; }