void TPositionForPenalty::command(World &world, int me,
Robot::RobotCommand &command,
bool debug)
{
//450mm
MyVector2d mypos = world.GetRobotPositionByID(me);
MyVector2d target;
double linex;
double penalty_goal_dir;
// figure out which penalty is going
if (world.restartWhoseKick() == World::OurBall)
{
//
//
linex = (PENALTY_SPOT - ROBOT_DEF_WIDTH_H);
penalty_goal_dir = 1.0;
}
else
{
//
linex = -(PENALTY_SPOT - ROBOT_DEF_WIDTH_H);
penalty_goal_dir = -1.0;
}
if (debug)
{
gui_debug_line(me, GDBG_TACTICS, MyVector2d(linex, -100), MyVector2d(linex, 100));
}
if ((linex - mypos.x) * penalty_goal_dir > 0)
{//
// need to halt here
gui_debug_printf(me, GDBG_TACTICS, "we are in position\n");
command.cmd = Robot::CmdPosition;
command.target = mypos;
command.velocity = MyVector2d(0, 0);
command.angle = 0; //((penalty_goal_dir > 0) ? M_PI : 0.0);
command.observation_type = OBS_EVERYTHING_BUT_ME(me);
command.goto_point_type = Robot::GotoPointMove;
}
else
{
//Y,
//???Y
gui_debug_printf(me, GDBG_TACTICS, "we need to move\n");
// choose our target point
target.set(linex, mypos.y);
if (debug)
{
gui_debug_line(me, GDBG_TACTICS, mypos, target);
}
command.cmd = Robot::CmdPosition;
command.target = target;
command.velocity = MyVector2d(0, 0);
command.angle = 0; //((penalty_goal_dir > 0) ? M_PI : 0.0);
command.observation_type = OBS_EVERYTHING_BUT_ME(me);
command.goto_point_type = Robot::GotoPointMove;
}
}
void warn(const char *fmt, ...) {
if (!print_on_warn && !debug_on_warn)
return;
va_list va;
va_start(va, fmt);
gui_debug_printf("Warning (%08x): ", arm.reg[15]);
gui_debug_vprintf(fmt, va);
gui_debug_printf("\n");
va_end(va);
if (debug_on_warn)
debugger(DBG_EXCEPTION, 0);
}
void error(const char *fmt, ...) {
va_list va;
va_start(va, fmt);
gui_debug_printf("Error (%08x): ", arm.reg[15]);
gui_debug_vprintf(fmt, va);
gui_debug_printf("\n");
va_end(va);
debugger(DBG_EXCEPTION, 0);
cpu_events |= EVENT_RESET;
#ifndef NO_SETJMP
__builtin_longjmp(restart_after_exception, 1);
#else
assert(false);
#endif
}
void backtrace(uint32_t fp) {
uint32_t *frame;
gui_debug_printf("Frame PrvFrame Self Return Start\n");
do {
gui_debug_printf("%08X:", fp);
frame = (uint32_t*) virt_mem_ptr(fp - 12, 16);
if (!frame) {
gui_debug_printf(" invalid address\n");
break;
}
//vgui_debug_printf(" %08X %08X %08X %08X\n", (void *)frame);
if (frame[0] <= fp) /* don't get stuck in infinite loop :) */
break;
fp = frame[0];
} while (frame[2] != 0);
}
static uint32_t parse_expr(char *str) {
uint32_t sum = 0;
int sign = 1;
if (str == NULL)
return 0;
while (*str) {
int reg;
if (isxdigit(*str)) {
sum += sign * strtoul(str, &str, 16);
sign = 1;
} else if (*str == '+') {
str++;
} else if (*str == '-') {
sign = -1;
str++;
} else if (*str == 'v') {
sum += sign * mmu_translate(strtoul(str + 1, &str, 16), false, NULL, NULL);
sign = 1;
} else if (*str == 'r') {
reg = strtoul(str + 1, &str, 10);
if(reg > 15)
{
gui_debug_printf("Reg number out of range!\n");
return 0;
}
sum += sign * arm.reg[reg];
sign = 1;
} else {
for (reg = 13; reg < 16; reg++) {
if (!memcmp(str, reg_name[reg], 2)) {
str += 2;
sum += sign * arm.reg[reg];
sign = 1;
goto ok;
}
}
gui_debug_printf("syntax error\n");
return 0;
ok:;
}
}
return sum;
}
static void disasm(uint32_t (*dis_func)(uint32_t pc)) {
char *arg = strtok(NULL, " \n\r");
uint32_t addr = arg ? parse_expr(arg) : arm.reg[15];
int i;
for (i = 0; i < 16; i++) {
uint32_t len = dis_func(addr);
if (!len) {
gui_debug_printf("Address %08X is not in RAM.\n", addr);
break;
}
addr += len;
}
}
static void dump(uint32_t addr) {
uint32_t start = addr;
uint32_t end = addr + 0x7F;
uint32_t row, col;
for (row = start & ~0xF; row <= end; row += 0x10) {
uint8_t *ptr = (uint8_t*) virt_mem_ptr(row, 16);
if (!ptr) {
gui_debug_printf("Address %08X is not in RAM.\n", row);
break;
}
gui_debug_printf("%08X ", row);
for (col = 0; col < 0x10; col++) {
addr = row + col;
if (addr < start || addr > end)
gui_debug_printf(" ");
else
gui_debug_printf("%02X", ptr[col]);
gui_debug_printf(col == 7 && addr >= start && addr < end ? "-" : " ");
}
gui_debug_printf(" ");
for (col = 0; col < 0x10; col++) {
addr = row + col;
if (addr < start || addr > end)
gui_debug_printf(" ");
else if (ptr[col] < 0x20)
gui_debug_printf(".");
else
{
char str[] = {(char) ptr[col], 0};
gui_debug_printf(str);
}
}
gui_debug_printf("\n");
}
}
void TPositionForKick::command(World &world, int me,
Robot::RobotCommand &command,
bool debug)
{
MyVector2d ball_position = world.ball_position();
MyVector2d robot_position = world.GetRobotPositionByID(me);
MyVector2d robot_velocity = world.GetRobotVelocityByID(me);
MyVector2d target;
double angle_tolerance;
if (!prev_target_set)
{
prev_target = world.their_goal;
prev_target_set = true;
}
//# The amount we'd prefer to shoot at our previous angle. If an another
//# at least this much bigger appears we'll switch to that.
//SHOOT_AIM_PREF_AMOUNT = 0.01745 # 1 degree
// (1) Try shooting on goal.
if (!evaluation.aim(world, world.now, ball_position,
world.their_goal_r,
world.their_goal_l,
OBS_EVERYTHING_BUT_US,
prev_target, DVAR(SHOOT_AIM_PREF_AMOUNT),
target, angle_tolerance))
{
//back border of the goal lu_test
MyVector2d downfield[2];
downfield[0].set(ball_position.x + 180.0, -FIELD_WIDTH_H);
downfield[1].set(ball_position.x + 180.0, FIELD_WIDTH_H);
// (2) Try clearing the ball
if (!evaluation.aim(world, world.now, ball_position,
downfield[0], downfield[1],
OBS_EVERYTHING_BUT_ME(me),
prev_target, DVAR(SHOOT_AIM_PREF_AMOUNT),
target, angle_tolerance))
{
// Guaranteed to return true and fill in the parameters when
// obs_flags is empty.
// (3) Just shoot downfield.
//
evaluation.aim(world, world.now, ball_position,
downfield[0], downfield[1],
0, target, angle_tolerance);
qDebug()<<"shoot downfield";
}
}
if (debug)
{
gui_debug_line(me, GDBG_TACTICS, ball_position, target);
gui_debug_line(me, GDBG_TACTICS, ball_position,
(target - ball_position).rotate(angle_tolerance) + ball_position);
gui_debug_line(me, GDBG_TACTICS, ball_position,
(target - ball_position).rotate(-angle_tolerance) + ball_position);
}
prev_target = target;
//
double ball_distance = (robot_position - ball_position).length();
//question lu_test
if (robot_velocity.length() < 20.0)
{
ball_distance -= 20.0;
}
// put this in config
double closest = 85.0;
//question lu_test constant setting
ball_distance = bound(ball_distance, closest, 150.0);
//target85150
MyVector2d targetp = ball_position - (target - ball_position).norm(ball_distance);
double angle = (ball_position - targetp).angle();
int obs = OBS_EVERYTHING_BUT_ME(me);
MyVector2d r2target = (targetp - robot_position);
double d2target = r2target.sqlength();
//20150
//question lu_test ???
if ((d2target < 150.0 * 150.0) && (d2target > 20.0 * 20.0) &&
(fabs(angle_mod(angle - r2target.angle())) < M_PI_4))
{
// obs = OBS_WALLS;
obs = 0;
// printf("turning off obstacle avoidance!!!\n");
if (debug)
{
gui_debug_printf(me, GDBG_TACTICS, "turning off obstacle avoidance!!!\n");
}
}
command.cmd = Robot::CmdPosition;
command.target = targetp;
command.velocity = MyVector2d(0, 0);
command.angle = angle;
//command.obs = OBS_EVERYTHING_BUT_ME(me);
command.observation_type = obs;
command.goto_point_type = Robot::GotoPointMoveForw;
}
// return 1: break (should stop being feed with debugger commands), 0: continue (can be feed with other debugger commands)
int process_debug_cmd(char *cmdline) {
char *cmd = strtok(cmdline, " \n\r");
if (!cmd)
return 0;
if (!strcasecmp(cmd, "?") || !strcasecmp(cmd, "h")) {
gui_debug_printf(
"Debugger commands:\n"
"b - stack backtrace\n"
"c - continue\n"
"d <address> - dump memory\n"
"k <address> <+r|+w|+x|-r|-w|-x> - add/remove breakpoint\n"
"k - show breakpoints\n"
"ln c - connect\n"
"ln s <file> - send a file\n"
"ln st <dir> - set target directory\n"
"mmu - dump memory mappings\n"
"n - continue until next instruction\n"
"pr <address> - port or memory read\n"
"pw <address> <value> - port or memory write\n"
"r - show registers\n"
"rs <regnum> <value> - change register value\n"
"ss <address> <length> <string> - search a string\n"
"s - step instruction\n"
"t+ - enable instruction translation\n"
"t- - disable instruction translation\n"
"u[a|t] [address] - disassemble memory\n"
"wm <file> <start> <size> - write memory to file\n"
"wf <file> <start> [size] - write file to memory\n"
"stop - stop the emulation\n"
"exec <path> - exec file with ndless\n");
} else if (!strcasecmp(cmd, "b")) {
char *fp = strtok(NULL, " \n\r");
backtrace(fp ? parse_expr(fp) : arm.reg[11]);
} else if (!strcasecmp(cmd, "mmu")) {
mmu_dump_tables();
} else if (!strcasecmp(cmd, "r")) {
int i, show_spsr;
uint32_t cpsr = get_cpsr();
const char *mode;
for (i = 0; i < 16; i++) {
int newline = ((1 << 5) | (1 << 11) | (1 << 15)) & (1 << i);
gui_debug_printf("%3s=%08x%c", reg_name[i], arm.reg[i], newline ? '\n' : ' ');
}
switch (cpsr & 0x1F) {
case MODE_USR: mode = "usr"; show_spsr = 0; break;
case MODE_SYS: mode = "sys"; show_spsr = 0; break;
case MODE_FIQ: mode = "fiq"; show_spsr = 1; break;
case MODE_IRQ: mode = "irq"; show_spsr = 1; break;
case MODE_SVC: mode = "svc"; show_spsr = 1; break;
case MODE_ABT: mode = "abt"; show_spsr = 1; break;
case MODE_UND: mode = "und"; show_spsr = 1; break;
default: mode = "???"; show_spsr = 0; break;
}
gui_debug_printf("cpsr=%08x (N=%d Z=%d C=%d V=%d Q=%d IRQ=%s FIQ=%s T=%d Mode=%s)",
cpsr,
arm.cpsr_n, arm.cpsr_z, arm.cpsr_c, arm.cpsr_v,
cpsr >> 27 & 1,
(cpsr & 0x80) ? "off" : "on ",
(cpsr & 0x40) ? "off" : "on ",
cpsr >> 5 & 1,
mode);
if (show_spsr)
gui_debug_printf(" spsr=%08x", get_spsr());
gui_debug_printf("\n");
} else if (!strcasecmp(cmd, "rs")) {
void gui_perror(const char *msg)
{
gui_debug_printf("%s: %s\n", msg, strerror(errno));
}
bool emu_start(unsigned int port_gdb, unsigned int port_rdbg, const char *snapshot_file)
{
gui_busy_raii gui_busy;
if(snapshot_file)
{
// Open snapshot
size_t snapshot_size = gzip_filesize(snapshot_file);
if(snapshot_size < sizeof(emu_snapshot))
return false;
FILE *fp = fopen_utf8(snapshot_file, "rb");
if(!fp)
return false;
int dupfd = dup(fileno(fp));
fclose(fp);
fp = nullptr;
// gzdopen takes ownership of the fd
gzFile gzf = gzdopen(dupfd, "r");
if(!gzf)
{
close(dupfd);
return false;
}
auto snapshot = (struct emu_snapshot *) malloc(snapshot_size);
if(!snapshot)
{
gzclose(gzf);
return false;
}
if((size_t) gzread(gzf, snapshot, snapshot_size) != snapshot_size)
{
gzclose(gzf);
free(snapshot);
return false;
}
gzclose(gzf);
//sched_reset();
sched.items[SCHED_THROTTLE].clock = CLOCK_27M;
sched.items[SCHED_THROTTLE].proc = throttle_interval_event;
// TODO: Max length
path_boot1 = std::string(snapshot->path_boot1);
path_flash = std::string(snapshot->path_flash);
// TODO: Pass snapshot_size to flash_resume to avoid reading after the buffer
// Resume components
uint32_t sdram_size;
if(snapshot->sig != SNAPSHOT_SIG
|| snapshot->version != SNAPSHOT_VER
|| !flash_resume(snapshot)
|| !flash_read_settings(&sdram_size, &product, &features, &asic_user_flags)
|| !cpu_resume(snapshot)
|| !memory_resume(snapshot)
|| !sched_resume(snapshot))
{
emu_cleanup();
free(snapshot);
return false;
}
free(snapshot);
}
else
{
if (!flash_open(path_flash.c_str()))
return false;
uint32_t sdram_size;
flash_read_settings(&sdram_size, &product, &features, &asic_user_flags);
flash_set_bootorder(boot_order);
if(!memory_initialize(sdram_size))
{
emu_cleanup();
return false;
}
}
if(debug_on_start)
cpu_events |= EVENT_DEBUG_STEP;
uint8_t *rom = mem_areas[0].ptr;
memset(rom, -1, 0x80000);
for (int i = 0x00000; i < 0x80000; i += 4)
RAM_FLAGS(&rom[i]) = RF_READ_ONLY;
/* Load the ROM */
FILE *f = fopen_utf8(path_boot1.c_str(), "rb");
if (!f) {
gui_perror(path_boot1.c_str());
emu_cleanup();
return false;
}
(void)fread(rom, 1, 0x80000, f);
fclose(f);
#ifndef NO_TRANSLATION
if(!translate_init())
{
gui_debug_printf("Could not init JIT, disabling translation.\n");
do_translate = false;
}
#endif
addr_cache_init();
throttle_timer_on();
if(port_gdb)
gdbstub_init(port_gdb);
if(port_rdbg)
rdebug_bind(port_rdbg);
usblink_queue_reset();
if(!snapshot_file)
emu_reset();
return true;
}
