void checkAndConnect(Hok_t *hok) {
if (!hok->isWorking) {
printf("%sHokuyo not connected, trying to connect to %s\n", PREFIX, hok->path);
int error = urg_connect(hok->urg, hok->path, 115200);
if (error < 0) {
printf("%sCan't connect to hokuyo : %s\n", PREFIX, urg_error(hok->urg));
hok->isWorking = 0;
} else {
hok->imin = urg_rad2index(hok->urg, hok->cone_min);
hok->imax = urg_rad2index(hok->urg, hok->cone_max);
urg_setCaptureTimes(hok->urg, UrgInfinityTimes);
error = urg_requestData(hok->urg, URG_MD, hok->imin, hok->imax);
if (error < 0) {
printf("%sCan't connect to hokuyo\n", PREFIX);
hok->isWorking = 0;
} else {
printf("%sHokuyo connected\n", PREFIX);
hok->isWorking = 1;
printf("%sRequesting data on indexes %d to %d from %s OK\n", PREFIX, hok->imin, hok->imax, hok->path);
hok->nb_data = urg_dataMax(hok->urg);
double *angles = malloc(hok->nb_data * sizeof(double));
int i;
for (i=0; i<hok->nb_data; i++) {
angles[i] = modTwoPi(urg_index2rad(hok->urg, i) + hok->orientation);
}
hok->fm = initFastmath(hok->nb_data, angles);
free(angles);
printf("%sCalculted sin/cos data for %s\n", PREFIX, hok->path);
}
}
}
}
int urg_rad2step(const urg_t *urg, double radian)
{
if (!urg->is_active) {
return URG_NOT_CONNECTED;
}
return urg_rad2index(urg, radian) - urg->front_data_index;
}
int urg_deg2index(const urg_t *urg, int degree)
{
return urg_rad2index(urg, M_PI * degree / 180.0);
}
void main_init() {
int i;
float starting_speed = 0.0;
init_robot();
flag = 0;
robot_state = malloc(sizeof (float) *3);
for (i = 0; i < LEN_PIC_BUFFER; i++) {
set_vel_2_array(pic_buffer[i], starting_speed, starting_speed);
}
write(pic_fd, pic_message_reset_steps_acc, PACKET_TIMING_LENGTH + 1);
tcflush(pic_fd, TCOFLUSH);
sync();
steps_anomaly = 0;
#ifdef TEST_KALMAN
float v=0;
float w=0;
set_robot_speed(&v,&w);
#endif
#ifdef OB_AV
init_probstavoid_module();
#endif
#ifdef CARTESIAN_REGULATOR
viapoints[0][0]=38;
viapoints[0][1]=119;
viapoints[0][2]=0;
viapoints[1][0]=98;
viapoints[1][1]=161;
viapoints[1][2]=0;
viapoints[2][0]=187;
viapoints[2][1]=179;
viapoints[2][2]=0;
viapoints[3][0]=158;
viapoints[3][1]=238;
viapoints[3][2]=0;
viapoints[4][0]=187;
viapoints[4][1]=268;
viapoints[4][2]=0;
curr_via=0;
via_done=0;
#endif
#ifdef HOKUYO
init_urg_laser(&urg,ON_DEMAND);
#endif
#ifdef JOYSTICK
init_joystick();
#endif
#ifdef EKF_LOC
load_map("ekf_map.txt");
init_ekf();
#ifdef HOKUYO_SENSOR
for (i=0; i< NUM_SENS; i++){
ANGLE_IDX[i]=urg_rad2index(urg,ANGLE_H[i]);
printf("angle: %f \tidx: %d\n",ANGLE_H[i],urg_rad2index(urg,ANGLE_H[i]));
}
#endif
xpost.x=50;
xpost.y=45;
xpost.th=M_PI/2;
xpred.x=0;
xpred.y=0;
xpred.th=0;
#endif
}
int urg_deg2index(const urg_t *urg, double degree)
{
return urg_rad2index(urg, degree * M_PI / 180.0);
}