static void solo_timer(int sig)
{
if((sm->gameflag) && (sm->sologame))
{
sm->sticks[sm->joynumber] = sm->ownstick; /* TEST */
move_all(sm->playfeld,sm->sticks);
}
}
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);
}
