/*-----------------------------------------------------------*/
bool NOMAD::SMesh::check_min_poll_size_criterion ( ) const
{
if ( !_Delta_min.is_defined() )
return false;
NOMAD::Point Delta;
return get_Delta ( Delta );
}
NOMAD::Double NOMAD::SMesh::scale_and_project(int i, NOMAD::Double l) const
{
NOMAD::Point delta;
NOMAD::Point Delta;
get_delta ( delta );
get_Delta ( Delta );
if ( delta.is_defined() && Delta.is_defined() && i <= _n)
{
NOMAD::Double d= Delta[i] / delta[i] * l;
return d.NOMAD::Double::round()*delta[i];
}
else
throw NOMAD::Exception ( "SMesh.cpp" , __LINE__ ,
"Mesh scaling and projection cannot be performed!" );
}
/**
* Checks for "param" command
* return 0 if the command was "param"
*/
int check_for_params(void){
if(veh.server_reply[0] == 'p'){
if (veh.state == VEH_INIT){
// set params
printf("**************************************************\n");
printf("PARAMETERS RECEIVED\n");
printf("SERVER REPLY: %s;;;\n",veh.server_reply);
int parsed;
parsed = sscanf(&veh.server_reply[2], "%f;%f;%f;%f", &veh.rx_delta_left_rad, &veh.rx_delta_right_rad, &veh.rx_omega_left_rad, &veh.rx_omega_right_rad);
printf("Parsed numbers = %d\n", parsed);
printf("PARSED VALUES: %f;%f;%f;%f;\n", veh.rx_delta_left_rad, veh.rx_delta_right_rad, veh.rx_omega_left_rad, veh.rx_omega_right_rad);
printf("**************************************************\n");
// check for values
//if ((veh.delta_left_rad==0) || (veh.delta_right_rad=0) || (veh.omega_left_rad==0) || (veh.omega_right_rad==0)) {
// printf("Received zero value for parameters.\n");
//}
//else {
// values OK
// Left frequency
printf("%s%f\n", "RX O_L: ", veh.rx_omega_left_rad);
printf("SETTING OMEGA LEFT PARAMETER\n");
#if !DEBUG
set_freq_b(LEFT, veh.rx_omega_left_rad, 0);
set_freq_b(LEFT, veh.rx_omega_left_rad, 1);
veh.omega_left_rad = get_Omega(LEFT, 0);
#else
veh.omega_left_rad = veh.rx_omega_left_rad;
#endif
printf("%s%f\n", "TX O_L: ", veh.omega_left_rad);
// Right frequency
printf("%s%f\n", "RX O_R: ", veh.rx_omega_right_rad);
printf("SETTING OMEGA right PARAMETER\n");
#if !DEBUG
set_freq_b(RIGHT, veh.rx_omega_right_rad, 0);
set_freq_b(RIGHT, veh.rx_omega_right_rad, 1);
veh.omega_right_rad = get_Omega(RIGHT, 0);
#else
veh.omega_right_rad = veh.rx_omega_right_rad;
#endif
printf("%s%f\n", "TX O_R: ", veh.omega_right_rad);
// Left delta
printf("%s%f\n", "RX D_L: ", veh.rx_delta_left_rad);
printf("SETTING DELTA LEFT PARAMETER\n");
#if !DEBUG
set_delta_b(LEFT, veh.rx_delta_left_rad, 0);
set_delta_b(LEFT, veh.rx_delta_left_rad, 1);
veh.delta_left_rad = get_Delta(LEFT, 0);
#else
veh.delta_left_rad = veh.rx_delta_left_rad;
#endif
printf("%s%f\n", "TX D_L: ", veh.delta_left_rad);
// Right delta
printf("%s%f\n", "RX D_R: ", veh.rx_delta_right_rad);
printf("SETTING DELTA RIGHT PARAMETER\n");
#if !DEBUG
set_delta_b(RIGHT, (double)veh.rx_delta_right_rad, 0);
set_delta_b(RIGHT, (double)veh.rx_delta_right_rad, 1);
veh.delta_right_rad = get_Delta(RIGHT, 0);
#else
veh.delta_right_rad = veh.rx_delta_right_rad;
#endif
printf("%s%f\n", "TX D_R: ", veh.delta_right_rad);
//}
}
else {
// vehicle not running
printf("**************************************************\n");
printf("Not ready (stopped or not initialized.\n");
printf("**************************************************\n");
}
// prepare response
sprintf(veh.message, "p;%f;%f;%f;%f;\n", veh.delta_left_rad, veh.delta_right_rad, veh.omega_left_rad, veh.omega_right_rad);
// command found
return 0;
}
else {
// continue search
return 1;
}
}
/**
\return Delta The poll size parameter Delta^k -- \b OUT.
*/
NOMAD::Point get_Delta ( void )
{
NOMAD::Point Delta;
get_Delta(Delta);
return Delta;
}
