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(void) {
InitPeripherals();
mRedOFF; mGreenOFF; mBlueOFF;
// startup blink
const float kShortDelay = 0.1;
const float kLongDelay = 0.3;
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
UsbInterface usb = UsbInterface();
usb.Init();
PersistentMemory mem;
DelaySeconds(1.0f);
ZigbeeInterface zig = ZigbeeInterface();
DelaySeconds(0.05f);
zig.Init(mem);
error_reporting_com1 = &usb;
error_reporting_com2 = &zig;
Mpu60XXImu imu(mem);
GyroAccelDriftUkfIo est;
BatteryMonitor battery(mem);
battery_ptr = &battery;
MotorHal motor_hal(mem);
motor_hal_ptr = &motor_hal;
// Get_pos addition:
InitPositionSensor();
tim1_set_position_callback_2(get_position);
QuatPD pd(mem);
PulsingCoaxControllerQuat uav(mem, pd, imu, est, motor_hal);
CoaxOpenController open_controller(mem);
CoaxOpenAttitudeController open_attitude_controller(mem, est);
monitor = LoopMonitor(mem);
UavReporter reporter(mem, imu, est, motor_hal, battery, uav, open_attitude_controller, pd, monitor);
StateMachine state_machine;
mem.Freeze(); // freeze memory to make writes possible
imu.InitFromMemory();
//imu.DefaultAccelSensitivity(1); // overwrite scale for off-datasheet fix
imu.flip_z = 1; // flip z-axis direction for upside-down imu
#ifdef STREAM_IMU_RAW
imu_raw_msg_logger_init(imu);
#endif
monitor.InitFromMemory();
PlayTimebase();
imu.StartRead();
DelayMilliseconds(10);
tim1_init();
tim1_set_supply_volts(3.7f);
//////////////////////////////////////////////////////////////////////////////
// Main Loop
while(1) {
monitor.Profile(0);
////////////////////////////////////////////////////////////////////////////
// Get IMU Data and Start New Measurement
while(!imu.FinishRead()) {};
#ifdef STREAM_IMU_RAW
imu_raw_msg_logger_push();
#endif
DelayMicroseconds(50); // this seems to be critical (?!?)
imu.StartRead();
monitor.Profile(1);
////////////////////////////////////////////////////////////////////////////
// Update Estimator with Measurement
est.Update(imu.time, imu.w, imu.a);
////////////////////////////////////////////////////////////////////////////
// Control
// update control laws
uav.Update();
open_controller.Update();
open_attitude_controller.Update();
// map controllers to outputs based on state
enum control_state state = state_machine.get_state();
// in STOP state, send active kill messages to motors
if(state == kStop) {
mGreenOFF; mAmberON;
motor_hal.set_top_cmd_volts(0);
motor_hal.set_top_cmd_volts_pulse_amp(0);
motor_hal.set_top_cmd_pulse_phase(0);
motor_hal.set_bottom_cmd_volts(0);
}
// in STANDBY state, send no motor commands except on entry
else if(state == kStandby) {
mGreenOFF; mAmberOFF;
if(state_machine.get_standby_needs_init()) {
motor_hal.set_top_cmd_volts(0);
motor_hal.set_top_cmd_volts_pulse_amp(0);
motor_hal.set_top_cmd_pulse_phase(0);
motor_hal.set_bottom_cmd_volts(0);
state_machine.clear_standby_needs_init();
}
}
// in QUAT state, send motor commands according to quat control law
else if(state == kQuat) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(uav.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(uav.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(uav.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(uav.bottom_mean);
}
// in OPEN state, send motor command according to open motor control commands
else if(state == kOpen) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(open_controller.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(open_controller.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(open_controller.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(open_controller.bottom_mean);
}
// in OPEN ATTITUDE state, send motor command according to open motor control commands
else if(state == kOpenAttitude) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(open_attitude_controller.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(open_attitude_controller.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(open_attitude_controller.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(open_attitude_controller.bottom_mean);
}
monitor.Profile(2);
////////////////////////////////////////////////////////////////////////////
// Packet Communication
uint8_t is_data; // 1 iff data received
uint8_t *rx_data; // temporary pointer to received type+data bytes
uint8_t rx_length; // number of received type+data bytes
////////////////////////////////////////////////////////////////////////////
// USB Input
is_data = 0;
usb.GetBytes();
while(usb.PeekPacket(&rx_data, &rx_length)) {
zig.ReadMsg(usb, rx_data, rx_length);
imu.ReadMsg(usb, rx_data, rx_length);
est.ReadMsg(usb, rx_data, rx_length);
mem.ReadMsg(usb, rx_data, rx_length);
uav.ReadMsg(usb, rx_data, rx_length);
open_controller.ReadMsg( usb, rx_data, rx_length);
open_attitude_controller.ReadMsg( usb, rx_data, rx_length);
pd.ReadMsg( usb, rx_data, rx_length);
monitor.ReadMsg( usb, rx_data, rx_length);
motor_hal.ReadMsg(usb, rx_data, rx_length);
battery.ReadMsg( usb, rx_data, rx_length);
reporter.ReadMsg( usb, rx_data, rx_length);
state_machine.ReadMsg(usb, rx_data, rx_length);
usb.DropPacket();
is_data = 1;
} // while peek...
if(is_data) {
usb.SendNow();
}
////////////////////////////////////////////////////////////////////////////
// Radio Input
is_data = 0;
zig.GetBytes();
while(zig.PeekPacket(&rx_data, &rx_length)) {
#ifdef STREAM_IMU_RAW
if(rx_data[0] == kTypeQuatPilot || rx_data[0] == kTypeQuatFullObsPilot || rx_data[0] == kTypeOpenPilot || rx_data[0] == kTypeOpenAttitudePilot) {
imu_raw_msg_logger_send(zig);
}
#endif
zig.ReadMsg(zig, rx_data, rx_length);
imu.ReadMsg(zig, rx_data, rx_length);
est.ReadMsg(zig, rx_data, rx_length);
mem.ReadMsg(zig, rx_data, rx_length);
uav.ReadMsg(zig, rx_data, rx_length);
open_controller.ReadMsg( zig, rx_data, rx_length);
open_attitude_controller.ReadMsg( zig, rx_data, rx_length);
pd.ReadMsg( zig, rx_data, rx_length);
monitor.ReadMsg( zig, rx_data, rx_length);
motor_hal.ReadMsg(zig, rx_data, rx_length);
battery.ReadMsg( zig, rx_data, rx_length);
reporter.ReadMsg( zig, rx_data, rx_length);
state_machine.ReadMsg(zig, rx_data, rx_length);
zig.DropPacket();
is_data = 1;
} // while peek...
monitor.Profile(3);
if(is_data) {
zig.SendNow();
}
monitor.Profile(4);
monitor.Profile(5);
////////////////////////////////////////////////////////////////////////////
// Throttle main loop to main_freq
monitor.EndMainLoop();
monitor.Profile(6);
//monitor.SendProfile(usb);
//usb.SendNow();
} // while(1)
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;
}
