int main(void) {
char c;
init_robot();
fflush(stdin);
printf("Usage: %c to exit, %c to move forward, %c to move backward, %c to rotate to left, %c to rotate to right!\n\n", EXIT_S, MF_S, MB_S, RL_S, RR_S);
print_world();
scanf("%c", &c);
while(c != EXIT_S) {
switch(c) {
case MF_S:
move_robot(MOVE_FWD);
print_world();
break;
case MB_S:
move_robot(MOVE_BWD);
print_world();
break;
case RL_S:
rotate_90_left();
print_world();
break;
case RR_S:
rotate_90_right();
print_world();
break;
}
scanf("%c", &c);
}
return 0;
}
/* 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);
}
}
int loop(struct robot r1, struct robot r2, int round){
if (round>=30){
printf("VINCENT has escaped!\n");
return 0;
}
else
round++;
char c = get_character();
if (c!='F' && c!='B' && c!='L' && c!='R'){
round--;
return loop(r1, r2, round);
}
else if (round%2==1){
r1 = move_robot(r1, c);
printf("VINCENT has made a ");
display(c);
printf(" to location (%d,%d) facing ", r1.location.x, r1.location.y);
display(r1.orientation);
printf(".\n");
}
else{
r2 = move_robot(r2, c);
printf("MAXIMILLIAN has made a ");
display(c);
printf(" to location (%d,%d) facing ", r2.location.x, r2.location.y);
display(r2.orientation);
printf(".\n");
}
if (round%2==0){
if (is_visible(r1, r2)){
printf("Facing ");
display(r1.orientation);
printf(", I see MAXIMILLIAN. Run!\n");
}
if (is_visible(r2, r1)){
printf("You can't run VINCENT. Exterminate!\n");
return 0;
}
}
return loop(r1, r2, round);
}
int main(void)
{
LED_INIT;
//servoTx;
sei();
USART_init();
MPU_init();
SERVO_init();
init_counters();
initvar();
SERVO_update_EEPROM(BROADCASTING_ID);
wait(10);
move_to_std();
wait_until_gyro_stable();
LED0_ON;
USART_send_message("Gyro Stable");
// ------ TESTCODE FOR READING SERVO -------
//servoGoto(1, 3.14/3, 0x200);
SERVO_update_EEPROM(BROADCASTING_ID); // NOTE: needs to run once for SERVO_get position to work
//----------------------------
reset_counter_1();
set_counter_1(3000);
uint8_t readyCounter = 0;
while(1)
{
MPU_update();
if(USART_get_turn_flag())
{
turn_degrees(USART_get_turn_angle(), USART_get_turn_dir());
}
if(USART_get_climb_flag())
{
climb();
}
uint8_t r = USART_getRotation();
uint8_t s = USART_getSpeed();
uint8_t d = USART_getDirection();
if(s != 0 || r != 50)
{
std_pos_flag = 0;
reset_counter_1();
readyCounter = 3;
}
move_robot(d, r, s);
if(r == 50 && s == 0 && d == 0 && readyCounter)
{
cli();
USART_send_ready();
sei();
readyCounter--;
}
if(move_to_std_flag == 1)
{
move_to_std_flag = 0;
move_to_std();
}
/*climb();
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
/*
change_z(-120);
move_to_std();
turn_degrees(180,1);
// Takes a predecided number of steps forward
// This is good when testing different things.
wait(100);
for(int i = 0; i < 10; ++i)
{
move_robot(0,50,100);
//wait(2000);
}
*/
USART_decode_rx_fifo();
}
}
int main(int argc, char* argv[])
{
p1u_setup();
printf("Test 1: No beeper\n");
fflush(stdout);
sleep(2);
enable_robot_debug_message();
char* world1[] = {
"wwww w",
"wwww w",
"wwww w",
" w",
"wwww w",
"wwww w"
};
create_robot_world_from_string(world1, 6, "test01q01", 1);
create_robot_at(0, 2);
move_robot();
p1world_shutdown();
P1U_ASSERT_EQUAL("Robot should be at X = 4", 4, get_current_robot_x());
P1U_ASSERT_EQUAL("Robot should be at Y = 0", 0, get_current_robot_y());
P1U_ASSERT_EQUAL("Robot should be facing south", get_south(), get_current_robot_orientation());
P1U_ASSERT_FALSE("Robot should survive", is_robot_dead());
printf("Test 2: Beeper present\n");
fflush(stdout);
sleep(2);
enable_robot_debug_message();
char* world2[] = {
"wwww w",
"wwww w",
"wwww w",
" bw",
"wwww w",
"wwww w"
};
create_robot_world_from_string(world2, 6, "test01q01", 1);
create_robot_at(0, 2);
move_robot();
p1world_shutdown();
P1U_ASSERT_EQUAL("Robot should be at X = 4", 4, get_current_robot_x());
P1U_ASSERT_EQUAL("Robot should be at Y = 5", 5, get_current_robot_y());
P1U_ASSERT_EQUAL("Robot should be facing north", get_north(), get_current_robot_orientation());
P1U_ASSERT_EQUAL("There should be one beeper at (4, 5)", 1, get_count_of_beepers_at(4, 5));
P1U_ASSERT_FALSE("Robot should survive", is_robot_dead());
p1u_shutdown();
return 0;
}
int goSomewhere(struct state *s, char **a, int penalty){
int i,j, is, js, k, wx=0, wy=0, stage=-1;
unsigned int change = 0, end = 0;
struct state *ns;
char * answer;
char move='U';
unsigned int *c = malloc( ((s-> world_w+1) * s->world_h+1) * sizeof( unsigned int ));
if (c == NULL ){}
for(i=0; i<((s-> world_w+1) * s->world_h+1); i++){
c[i]=UINT_MAX;
}
c[point_to_index(s, s->robot_x, s->robot_y)]=0;
put(s, 1, 1, O_EMPTY);
move_robot(s, 1, 1);
ns = copy(s);
do{
free(s);
s = copy(ns);
update_world(ns, s);
change++;
stage++;
end = 0;
for(i=1; i <= s->world_w; i++){
for(j=1; j <= s->world_h; j++){
if(c[point_to_index(s,i,j)]==stage){
if(get(s, i, j) == O_LIFT_OPEN || get(s, i, j) == O_LAMBDA){
wx=i;
wy=j;
end=stage;
break;
}
//consider four cells - (-1, 0), (1, 0), (0, -1), (0, 1)
for(k = 1; k<=4; k++){
if(k==1) {is= 0 ; js=-1;}
if(k==2) {is= 0 ; js= 1;}
if(k==3) {is=-1 ; js= 0;}
if(k==4) {is= 1 ; js= 0;}
if(bounded(s,i+is,j+js) && c[point_to_index(s, i+is, j+js)]==UINT_MAX){
//can we go there?
if( !bounded(s, i+is, j+js+1) || (get(s, i+is, j+js+1) != O_EMPTY || get(ns, i+is, j+js+1) != O_ROCK)){
if(get(s, i+is, j+js)==O_LIFT_OPEN
|| get(s, i+is, j+js)==O_LAMBDA
|| get(s, i+is, j+js)==O_EARTH
|| get(s, i+is, j+js)==O_EMPTY){
if((bounded(s, i+is, j+js+1) && get(s, i+is, j+js+1)==O_ROCK)
|| (bounded(s, i+is+1, j+js+1) && get(s, i+is+1, j+js)==O_ROCK && get(s, i+is+1, j+js+1)==O_ROCK)
|| (bounded(s, i+is-1, j+js+1) && get(s, i+is-1, j+js)==O_ROCK && get(s, i+is-1, j+js+1)==O_ROCK)){
c[point_to_index(s, i+is, j+js)]=stage+penalty;
}else{
c[point_to_index(s, i+is, j+js)]=stage+1;
}
change = 0;
}
}
}
}
}
}
}
}while(change <= penalty && end == 0);
//debug
//dump(s);
if(0){
for(j=s->world_h; j>0; j--){
for(i=1; i<=s->world_w; i++){
if(c[point_to_index(s, i, j)]==UINT_MAX){
printf("X");
}else{
printf("%u", c[point_to_index(s, i, j)]);
}
}
printf("\n");
}
printf("\n");
}
i=0;
if(end>0){
answer = malloc( (end+2)*sizeof( char ));
if (answer == NULL ){}
while(end>0){
for(k=1; k<=4;k++){
if(k==1) {is=-1; js= 0;move='R';}
if(k==2) {is= 1; js= 0;move='L';}
if(k==3) {is= 0; js=-1;move='U';}
if(k==4) {is= 0; js= 1;move='D';}
if( bounded(s, wx+is, wy+js) && c[point_to_index(s, wx+is, wy+js)] < end) {
end = c[point_to_index(s, wx+is, wy+js)];
answer[i++]=move;
wx = wx+is;
wy = wy+js;
break;
}
}
}
answer[i]='\0';
reverse(answer,0,strlen(answer)-1);
}else{
answer = malloc(1*sizeof( char ));
if (answer == NULL ){}
answer[0]='\0';
*a=answer;
free(c);
return 1;
}
*a=answer;
free(c);
return 0;
}
