bool filter::Move_to(int next_node){
int motor_one_status, motor_two_status, motor_one_steps, motor_two_steps;
int dir = 1;
int steps_to=distance_to[current_node][next_node]; //
if(steps_to <=0) // checks the direction
{
dir = 0;
steps_to= -1*steps_to;
}
std::cout<<steps_to<<" " << distance_to[current_node][next_node]<< std::endl;
int time_interval=10; // time interval between ticks the motor 10 ms
int reply; //used to call methods from stepper_bee_interface
reply = init_controller(); // turns the board
reply = set_modes(1,1,1,1); // allows the board to report back to use
reply = move_motor(1, dir, steps_to, time_interval, 0, 0, 0); // moves motor stepper_bee_interface just change the first int from a 1 to a 0
int wait=0; //built in for the delay that will occur on the usb
do{
reply = get_motor_status(&motor_one_status, &motor_two_status, &motor_one_steps, &motor_two_steps) ;
std::cout<<"MOTOR - steps left = "<< motor_two_steps<< " \n"; //CTM: change to motor_one_steps if using other motor
j++;
}while(motor_one_status == 1 || motor_two_status == 1|| wait<5); //this how we wait for the motor
std::cout<<"DONE !! \n";
current_node=next_node; // so the class knows where it is at
reply=close_controller(); //prevent bad signals being sent to the motor
return true;
}
int main(int argc, char * argv[])
{
struct playfield * field = malloc(HEIGHT * WIDTH *
sizeof(struct playfield));
struct player pl[4];
struct monster mons[MONSTERS];
struct bomb bm[(3 * WIDTH * HEIGHT) / 4];
struct controller_handle * ch;
struct command cmd;
struct bl_timer * timer;
int frame = 0;
int n_bombs = 0;
int n_monsters = MONSTERS;
int mx[4], my[4];
int last_ctype = 0;
int p = 0;
int i, j, k, l;
ch = open_controller(CONTROLLER_TYPE_STDIN);
if (!ch) {
fprintf(stderr, "No joysticks/controllers found.\n");
return -1;
}
srand(time(NULL));
for (i=0; i<PLAYERS; i++) {
mx[i] = 0;
my[i] = 0;
pl[i].fire = BEGIN_FIRE;
pl[i].max_bombs = BEGIN_BOMBS;
pl[i].cur_bombs = 0;
pl[i].has_red_bombs = 0;
pl[i].alive = 1;
pl[i].last_move = 0;
}
for (i=0; i<MONSTERS; i++) {
mons[i].x = WIDTH-1;
mons[i].y = 0;
mons[i].speed = 10;
mons[i].alive = 1;
}
pl[0].x = 0;
pl[0].y = 0;
pl[1].x = WIDTH - 1 - ((WIDTH+1) % 2);
pl[1].y = HEIGHT - 1;
pl[2].x = WIDTH - 1 - ((WIDTH+1) % 2);
pl[2].y = 0;
pl[3].x = ((HEIGHT+1) % 2);
pl[3].y = HEIGHT - 1;
populate_playfield(field, mons, n_monsters);
timer = bl_timer_create();
while(1) {
cmd = read_command(ch, DELAY);
if (cmd.controller >= 0)
p = cmd.controller;
if ((cmd.type == 2 && cmd.number == KEY_QUIT) ||
(cmd.type == IN_TYPE_STDIN && cmd.number == KEY_QUIT_K))
goto exit;
if (cmd.type > 0)
last_ctype = cmd.type;
if (cmd.controller >= 0) {
switch (cmd.number) {
case KEY_UP_K1:
case KEY_UP_K2:
case KEY_UP:
if (cmd.value > 0) {
mx[p] = 0;
my[p] = -1;
} else {
mx[p] = 0;
my[p] = 0;
pl[p].last_move = 0;
}
break;
case KEY_DOWN_K1:
case KEY_DOWN_K2:
case KEY_DOWN:
if (cmd.value > 0) {
mx[p] = 0;
my[p] = 1;
} else {
mx[p] = 0;
my[p] = 0;
pl[p].last_move = 0;
}
break;
case KEY_LEFT_K1:
case KEY_LEFT_K2:
case KEY_LEFT:
if (cmd.value > 0) {
mx[p] = -1;
my[p] = 0;
} else {
mx[p] = 0;
my[p] = 0;
pl[p].last_move = 0;
}
break;
case KEY_RIGHT_K1:
case KEY_RIGHT_K2:
case KEY_RIGHT:
if (cmd.value > 0) {
mx[p] = 1;
my[p] = 0;
} else {
mx[p] = 0;
my[p] = 0;
pl[p].last_move = 0;
}
break;
case KEY_B1_K1:
case KEY_B1_K2:
case KEY_B1:
if (pl[p].alive && pl[p].cur_bombs < pl[p].max_bombs &&
!is_bomb(pl[p].x, pl[p].y, bm, n_bombs)) {
bm[n_bombs].x = pl[p].x;
bm[n_bombs].y = pl[p].y;
bm[n_bombs].ticks = DEFAULT_BOMB_TICKS;
bm[n_bombs].is_red = pl[p].has_red_bombs;
bm[n_bombs].length = pl[p].fire;
bm[n_bombs].max_length[0] = bm[n_bombs].length;
bm[n_bombs].max_length[1] = bm[n_bombs].length;
bm[n_bombs].max_length[2] = bm[n_bombs].length;
bm[n_bombs].max_length[3] = bm[n_bombs].length;
bm[n_bombs].current_length = 0;
bm[n_bombs].from_player = p;
pl[p].cur_bombs++;
n_bombs++;
}
break;
default:
break;
}
}
if (bl_timer_elapsed(timer) > DELAY) {
for (i=0; i<n_monsters; i++) {
if (frame % mons[i].speed == 0) {
int dx, dy, nx, ny, npos;
int dir = rand() % 4;
npos = get_newpos_from_i(mons[i].x, mons[i].y, &dx, &dy, dir);
nx = mons[i].x + dx;
ny = mons[i].y + dy;
if (nx >= 0 && nx < WIDTH &&
ny >= 0 && ny < HEIGHT &&
field[npos].type == FIELD_TYPE_EMPTY) {
if (!is_bomb(nx, ny, bm, n_bombs)) {
mons[i].x = nx;
mons[i].y = ny;
}
}
}
}
for (i=0; i<PLAYERS; i++) {
if (frame > pl[i].last_move + 1) {
int nx, ny, npos;
nx = pl[i].x + mx[i];
ny = pl[i].y + my[i];
npos = ny * WIDTH + nx;
if (nx >= 0 && nx < WIDTH &&
ny >= 0 && ny < HEIGHT &&
field[npos].type == FIELD_TYPE_EMPTY) {
if (!is_bomb(nx, ny, bm, n_bombs)) {
if (pl[i].alive &&
field[npos].special != FIELD_SPECIAL_NONE) {
switch (field[npos].special) {
case FIELD_SPECIAL_FIRE:
pl[i].fire++;
break;
case FIELD_SPECIAL_BOMB:
pl[i].max_bombs++;
break;
case FIELD_SPECIAL_RED:
pl[i].has_red_bombs = 1;
break;
default:
break;
}
field[npos].special = FIELD_SPECIAL_NONE;
}
pl[i].x = nx;
pl[i].y = ny;
}
}
if (mx[i] != 0 || my[i] != 0) {
pl[i].last_move = frame;
}
}
}
for (i=0; i<n_bombs; i++) {
if (bm[i].current_length == bm[i].length) {
pl[bm[i].from_player].cur_bombs--;
n_bombs--;
for (j=i; j<n_bombs; j++) {
bm[j] = bm[j+1];
}
}
}
for (i=0; i<n_bombs; i++)
{
if (bm[i].ticks == 0) {
if (bm[i].current_length < bm[i].length) {
bm[i].current_length++;
}
} else {
if (frame % 3 == 0) {
bm[i].ticks--;
#ifdef RUMBLE
if (bm[i].ticks == 0) {
for (j=0; j<PLAYERS; j++) {
double power;
double dist = sqrt(pow((bm[i].x - pl[j].x), 2) +
pow((bm[i].y - pl[j].y), 2));
power = dist / MAX_DISTANCE;
if (power > 0.2) {
joyrumble(j+1, 100, 100, 500);
}
}
}
#endif
}
}
if (bm[i].ticks == 0)
{
for (j=0; j<4; j++) {
int dx, dy;
int pos = get_newpos_from_i(bm[i].x, bm[i].y, &dx, &dy, j);
if (field[pos].type != FIELD_TYPE_HARDWALL) {
for (k=0; k<bm[i].current_length; k++) {
int nx = bm[i].x + k * dx;
int ny = bm[i].y + k * dy;
if (nx >= 0 && nx < WIDTH &&
ny >= 0 && ny < HEIGHT) {
int npos = ny * WIDTH + nx;
if (k >= bm[i].max_length[j])
break;
if (!bm[i].is_red &&
field[npos].type == FIELD_TYPE_WALL) {
bm[i].max_length[j] = k + 1;
}
for (l=0; l<PLAYERS; l++) {
if (pl[l].x == nx && pl[l].y == ny) {
pl[l].alive = 0;
}
}
for (l=0; l<n_monsters; l++) {
if (mons[l].x == nx && mons[l].y == ny) {
mons[l].alive = 0;
}
}
for (l=0; l<n_bombs; l++)
{
if (bm[l].x == nx && bm[l].y == ny &&
bm[l].ticks > 1) {
bm[l].ticks = 1;
}
}
if (field[npos].type == FIELD_TYPE_EMPTY &&
k == bm[i].current_length - 1) {
field[npos].special = FIELD_SPECIAL_NONE;
}
field[npos].type = FIELD_TYPE_EMPTY;
}
}
}
}
}
}
for (i=0; i<n_monsters; i++) {
for (j=0; j<PLAYERS;j++) {
if (mons[i].alive &&
mons[i].x == pl[j].x &&
mons[i].y == pl[j].y)
{
pl[j].alive = 0;
}
}
}
draw_field(field, pl, bm, n_bombs, mons, n_monsters);
frame++;
bl_timer_start(timer);
if (last_ctype == IN_TYPE_STDIN) {
mx[p] = 0;
my[p] = 0;
pl[p].last_move = 0;
}
last_ctype = 0;
}
}
exit:
free(field);
close_controller(ch);
return 0;
}
int main(int argc, char * argv[]) {
struct coord snake[DISP_SIZE];
struct coord dir;
struct coord food;
struct controller_handle * ch;
struct command cmd;
struct bl_timer * timer;
struct snake_config conf;
int i;
int snake_length;
int cmd_queue[CMD_QUEUE_SIZE];
int cmdq_read = 0;
int cmdq_write = 0;
int cmdq_last = 0;
int n_cmdq = 0;
conf.delay = 100000;
conf.rotate_x = 0;
conf.rotate_y = 0;
// Read configuration
if (!read_config(config_handler, &conf, "snake")) {
fprintf(stderr, "Error reading configuration\n");
return 1;
}
srand (time(NULL));
memset(snake, 0, DISP_SIZE * sizeof(struct coord));
snake[0].x = WIDTH / 2;
snake[0].y = HEIGHT / 2;
snake[0].x = WIDTH / 2 + 1;
snake[0].y = HEIGHT / 2;
snake_length = 2;
dir.x = -1;
dir.y = 0;
food = new_food(snake, snake_length);
ch = open_controller(CONTROLLER_TYPE_JOYSTICK |
CONTROLLER_TYPE_STDIN);
if (!ch) {
fprintf(stderr, "No joysticks/controllers found.\n");
return -1;
}
timer = bl_timer_create();
while(1) {
int n_wait = conf.delay - bl_timer_elapsed(timer);
if (n_wait < 1)
n_wait = 1;
cmd = read_command(ch, n_wait);
if (((cmd.number >= 'a' && cmd.number <= 'w') ||
(cmd.number >= 8 && cmd.number <= 11)) &&
cmd.number != cmdq_last &&
(cmd.value == 1 || cmd.value > 10000) &&
!n_cmdq < CMD_QUEUE_SIZE) {
fprintf(stderr, "Cmd: %d\r\n", cmd.number);
cmd_queue[cmdq_write++ % CMD_QUEUE_SIZE] =
cmd.number;
n_cmdq++;
cmdq_last = cmd.number;
}
if (bl_timer_elapsed(timer) < conf.delay)
continue;
if (n_cmdq > 0) {
switch(cmd_queue[cmdq_read++ % CMD_QUEUE_SIZE]) {
case KEY_UP:
case KEY_UP_K1:
if (dir.y == 0) {
dir.x = 0;
dir.y = -1;
}
break;
case KEY_DOWN:
case KEY_DOWN_K1:
if (dir.y == 0) {
dir.x = 0;
dir.y = 1;
}
break;
case KEY_LEFT:
case KEY_LEFT_K1:
if (dir.x == 0) {
dir.x = -1;
dir.y = 0;
}
break;
case KEY_RIGHT:
case KEY_RIGHT_K1:
if (dir.x == 0) {
dir.x = 1;
dir.y = 0;
}
break;
case KEY_QUIT:
case KEY_QUIT_K:
goto exit;
default:
break;
}
n_cmdq--;
}
if (snake[0].x == food.x &&
snake[0].y == food.y) {
snake_length++;
snake[snake_length] = snake[snake_length-1];
food = new_food(snake, snake_length);
}
for (i=snake_length-1; i>=1; --i)
snake[i] = snake[i-1];
snake[0].x += dir.x;
snake[0].y += dir.y;
if (conf.rotate_x) {
if (snake[0].x < 0)
snake[0].x = WIDTH - 1;
else if (snake[0].x >= WIDTH)
snake[0].x = 0;
}
if (conf.rotate_y) {
if (snake[0].y < 0)
snake[0].y = HEIGHT - 1;
else if (snake[0].y >= HEIGHT)
snake[0].y = 0;
}
if (snake[0].x < 0 || snake[0].x >= WIDTH ||
snake[0].y < 0 || snake[0].y >= HEIGHT) {
fprintf(stderr, "Wand du trottel\n\r");
goto exit;
}
for (i=1; i<snake_length; ++i) {
if (snake[0].x == snake[i].x &&
snake[0].y == snake[i].y) {
fprintf(stderr, "Nom nom nom\n\r");
goto exit;
}
}
send_field(snake, snake_length, food);
paint_field(snake, snake_length, food);
bl_timer_start(timer);
}
exit:
bl_timer_free(timer);
close_controller(ch);
return 0;
}