CAMLprim value ml_gsl_stats_minmax(value data)
{
size_t len = Double_array_length(data);
double mi, ma;
gsl_stats_minmax(&mi, &ma, Double_array_val(data), 1, len);
return copy_two_double(mi, ma);
}
void function_initial_estimate( const double* pdX, const double* pdY, int iLength, double* pdParameterEstimates ) {
double dMin;
double dMax;
gsl_stats_minmax( &dMin, &dMax, pdX, 1, iLength );
pdParameterEstimates[0] = gsl_stats_mean( pdX, 1, iLength );
pdParameterEstimates[1] = ( dMax - dMin ) / 2.0;
pdParameterEstimates[2] = gsl_stats_max( pdY, 1, iLength );
}
int main (int argc, char * argv[]) {
parse_opts(argc,argv);
if(!doInit()) {
printf("Error Initializing Stuff.\n");
exit(EXIT_FAILURE);
} else { //o calibrar colores o seguir la linea
//bool rotate = true;
double pval;
bool stop = true;
int left, right;
while(!ag_psh_is_pushed(&push, &pval));
UDELAY(750000);
printf("pval is: %.2f\n", pval);
ag_lgt_set_led(&lright, true);
ag_lgt_set_led(&lleft, true);
if (mode) { //calibrar colores
int i = 0;
double acumr [calib], minr, maxr, upr, lowr;
double acuml [calib], minl, maxl, upl, lowl;
for (;i<calib;i++) {
acumr[i] = (double)ag_read_int(&lright);
acuml[i] = (double)ag_read_int(&lleft);
UDELAY(100);
}
gsl_stats_minmax(&minr, &maxr, acumr, 1, calib);
gsl_stats_minmax(&minl, &maxl, acuml, 1, calib);
gsl_sort (acumr,1,calib);
gsl_sort (acuml,1,calib);
upl = gsl_stats_quantile_from_sorted_data (acuml,1,calib,0.95);//uq
lowl = gsl_stats_quantile_from_sorted_data (acuml,1,calib,0.05);//lq
upr = gsl_stats_quantile_from_sorted_data (acumr,1,calib,0.95);//uq
lowr = gsl_stats_quantile_from_sorted_data (acumr,1,calib,0.05);//lq
for (i = 0 ; i < 2; i++){
printf("COLOR: %s, SENSOR: %s\n", white ? "WHITE" : "BLACK", i == 0 ? "LEFT" : "RIGHT");
printf("min_v: %d, max_v: %d\n", i == 0 ? (int)minl : (int)minr, i == 0 ? (int)maxl : (int)maxr);
printf("low_q: %d, up_q :%d\n", i == 0 ? (int)lowl : (int)lowr, i == 0 ? (int)upl : (int)upr);
printf("\n");
}
} else { //seguir linea
while(!ag_psh_is_pushed(&push, &pval)) {
//printf("pval is: %.2f\n", pval);
if (stop){
stop = false;
move_all(MY_FWD,vel);
}
right = ag_read_int(&lright);
left = ag_read_int(&lleft);
if(! IS_WHITE(right) || ! IS_WHITE(left)){
stop_all();
stop = true;
if (IS_BLACK(left)){
rotate_robot(vel, true);
while(IS_BLACK(ag_read_int(&lleft)));
UDELAY(TDELAY);
stop_all();
} else if (IS_BLACK(right)) {
rotate_robot(vel, false);
while(IS_BLACK(ag_read_int(&lright)));
UDELAY(TDELAY);
stop_all();
}
}
}
}
}
lego_shutdown();
exit(EXIT_SUCCESS);
}
pure_expr* wrap_gsl_stats_minmax(double* data, size_t n)
{
double x, y;
gsl_stats_minmax(&x, &y, data, 1, n);
return pure_listl(2, pure_double(x), pure_double(y));
}
