/* Reads and execute robot commands from FILE
*
* return 0 if fail
*/
int execute_from_file(char* file)
{
int i;
// Init COMMANDS_QUEUE
COMMANDS_QUEUE = queue_new();
if( ! fread_commands(file) ){ return 0; }
command_t* command = (command_t*) malloc(sizeof(command_t));
command = (command_t*) dequeue(COMMANDS_QUEUE);
while( command != NULL )
{
if( command -> command == C_STEP )
{
usleep(400000);
move_robot();
}
else if( command -> command == C_ROTATE )
{
if( command -> parameter_char == 'r' )
{
rotate_robot(RIGHT);
}
else
{
rotate_robot(LEFT);
}
}
else if( command -> command == C_JUMP )
{
for(i=0; i < (command -> parameter_int); i++)
{
usleep(400000);
move_robot();
}
}
else if( command -> command == C_TELEPORT )
{
teleport_robot();
}
else if( command -> command == C_ORIGIN )
{
move_robot_origin();
}
command = (command_t*) dequeue(COMMANDS_QUEUE);
}
}
void draw_robot(struct Vertex position, double heading)
{
translate_robot(position);
rotate_robot(heading);
draw_bumper();
draw_wheels();
draw_body();
}
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);
}
