Change sign for writing into file.
Update output for user. (show traveled distance)
robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
0);
robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
0);
- robot_move_by(-1.1, NO_TURN(), &tcSlow);
+ robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
FSM_TIMER(200);
break;
case EV_MOTION_DONE:
FSM_TIMER(200);
break;
case EV_MOTION_DONE:
x1 = robot.odo_distance_a;
y1 = robot.odo_distance_b;
ROBOT_UNLOCK(est_pos_odo);
x1 = robot.odo_distance_a;
y1 = robot.odo_distance_b;
ROBOT_UNLOCK(est_pos_odo);
+ printf("Distance for calibration: \n");
+ printf("Left%f\n", x1);
+ printf("Right%f\n", y1);
FILE * file;
file = fopen ("odometry_cal_data","a+");
FILE * file;
file = fopen ("odometry_cal_data","a+");
- sprintf(buffer, "%4.4f", 1/x1);
+ sprintf(buffer, "%4.4f", -1/x1);
fputs (buffer,file);
fputs ("\n", file);
fputs (buffer,file);
fputs ("\n", file);
- sprintf(buffer, "%4.4f", 1/y1);
+ sprintf(buffer, "%4.4f", -1/y1);
fputs (buffer,file);
fclose(file);
SUBFSM_RET(NULL);
fputs (buffer,file);
fclose(file);
SUBFSM_RET(NULL);