/* Move cursor to the first column in ls-view sub-mode selecting or unselecting
* files while moving. */
static void
cmd_zero(key_info_t key_info, keys_info_t *keys_info)
{
if(!at_first_column(view))
{
goto_pos(get_start_of_line(view));
}
}
void tenpercent(int sign)
{
static long topos;
topos=screen_offset+buffer_offset + sign*(long)(length-header_length)/10;
if(topos<(long)header_length) topos=header_length;
if(topos>length) topos=length;
goto_pos(topos);
}
/* Move cursor to the first column in ls-view sub-mode selecting or unselecting
* files while moving. */
static void
cmd_zero(key_info_t key_info, keys_info_t *keys_info)
{
if(!fpos_at_first_col(view))
{
goto_pos(fpos_line_start(view));
}
}
/* Move cursor to the last column in ls-view sub-mode selecting or unselecting
* files while moving. */
static void
cmd_dollar(key_info_t key_info, keys_info_t *keys_info)
{
if(!at_last_column(view))
{
goto_pos(get_end_of_line(view));
}
}
/* Move cursor to the last column in ls-view sub-mode selecting or unselecting
* files while moving. */
static void
cmd_dollar(key_info_t key_info, keys_info_t *keys_info)
{
if(!fpos_at_last_col(view))
{
goto_pos(fpos_line_end(view));
}
}
static void
cmd_ctrl_u(key_info_t key_info, keys_info_t *keys_info)
{
if(fpos_can_move_up(view))
{
goto_pos(fpos_half_scroll(view, 0));
}
}
static void
cmd_G(key_info_t key_info, keys_info_t *keys_info)
{
if(key_info.count == NO_COUNT_GIVEN)
{
key_info.count = view->list_rows;
}
goto_pos(key_info.count - 1);
}
static void
cmd_quote(key_info_t key_info, keys_info_t *keys_info)
{
const int pos = check_mark_directory(view, key_info.multi);
if(pos >= 0)
{
goto_pos(pos);
}
}
static void
cmd_quote(key_info_t key_info, keys_info_t *keys_info)
{
int pos;
pos = check_mark_directory(view, key_info.multi);
if(pos < 0)
return;
goto_pos(pos);
}
static void
cmd_G(key_info_t key_info, keys_info_t *keys_info)
{
int new_pos;
if(key_info.count == NO_COUNT_GIVEN)
key_info.count = view->list_rows;
new_pos = ROUND_DOWN(key_info.count - 1, view->column_count);
goto_pos(new_pos);
}
//! [0]
Window::Window(QString filename):run_animation(false),m_animation_number(0),anim_time(0.0)
{
scene = new GraphSceneJM(filename,this);
graph_view = new GraphViewJM(scene);
m_shotbutton = new QPushButton("Shot", this);
m_saveposbutton = new QPushButton("Save Pos", this);
m_newposbutton = new QPushButton("New Pos", this);
m_newposname_edit = new QLineEdit(this);
m_delposbutton = new QPushButton("Delete this position", this);
m_gotoposbutton = new QPushButton("Goto Pos:", this);
m_pos_list = new QComboBox(this);
m_destpos_list = new QComboBox(this);
m_animfile_label = new QLabel("Save as:",this);
m_animfile_edit = new QLineEdit(this);
m_writefilebutton = new QPushButton("Write File", this);
QGridLayout *mainLayout = new QGridLayout;
mainLayout->addWidget(graph_view,1,1,10,2);
mainLayout->addWidget(m_shotbutton,1,3,1,2);
mainLayout->addWidget(m_saveposbutton,2,3,1,2);
mainLayout->addWidget(m_pos_list,3,3,1,2);
mainLayout->addWidget(m_delposbutton,4,3,1,2);
mainLayout->addWidget(m_newposbutton,5,3,1,1);
mainLayout->addWidget(m_newposname_edit,5,4,1,1);
mainLayout->addWidget(m_gotoposbutton,6,3,1,1);
mainLayout->addWidget(m_destpos_list,6,4,1,1);
mainLayout->addWidget(m_animfile_label,7,3,1,1);
mainLayout->addWidget(m_animfile_edit,7,4,1,1);
mainLayout->addWidget(m_writefilebutton,8,3,1,2);
setLayout(mainLayout);
setWindowTitle(tr("QAnimDrawer"));
QObject::connect(m_shotbutton, SIGNAL(clicked()), graph_view, SLOT(ask_shot()));
QObject::connect(m_saveposbutton, SIGNAL(clicked()), this, SLOT(save_pos()));
QObject::connect(m_delposbutton, SIGNAL(clicked()), this, SLOT(del_pos()));
QObject::connect(m_gotoposbutton, SIGNAL(clicked()), this, SLOT(goto_pos()));
QObject::connect(m_newposbutton, SIGNAL(clicked()), this, SLOT(new_pos()));
QObject::connect(m_pos_list, SIGNAL(currentIndexChanged(int)), this, SLOT(change_pos(int)));
QObject::connect(m_writefilebutton, SIGNAL(clicked()), this, SLOT(write_file()));
scene->draw_skel();
update_pos_list();
timer=new QTimer(this);
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timer_timeout()));
}
static char *virtual_extension(buffer * const b) {
if (virt_ext == NULL) return NULL;
/* If the buffer filename has an extension, check that it's in extra_ext. */
const char * const filename_ext = extension(b->filename);
if (filename_ext != NULL) {
int i;
for(i = 0; i < num_extra_exts; i++)
if (fnmatch(extra_ext[i], filename_ext, 0) == 0) break;
if (i == num_extra_exts) return NULL;
}
/* Reduce the maximum number of lines to scan so that no more
than REGEX_SCAN_LIMIT characters are regex'd. */
int64_t line_limit = 0, pos_limit = -1, len = 0;
for(line_desc *ld = (line_desc *)b->line_desc_list.head; ld->ld_node.next && line_limit < max_max_line;
ld = (line_desc *)ld->ld_node.next, line_limit++)
if ((len += ld->line_len + 1) > REGEX_SCAN_LIMIT) {
line_limit++;
pos_limit = REGEX_SCAN_LIMIT - (len - ld->line_len - 1);
break;
}
int64_t earliest_found_line = INT64_MAX;
char *ext = NULL;
const int64_t b_cur_line = b->cur_line;
const int64_t b_cur_pos = b->cur_pos;
const int b_search_back = b->opt.search_back;
const int b_case_search = b->opt.case_search;
const int b_last_was_regexp = b->last_was_regexp;
char * const find_string = b->find_string;
b->opt.search_back = true;
for(int i = 0; earliest_found_line > 0 && i < num_virt_ext && !stop; i++) {
int64_t min_line = -1; /* max_line is 1-based, but internal line numbers (min_line) are 0-based. */
/* Search backwards in b from max_line for the first occurance of regex. */
b->opt.case_search = virt_ext[i].case_sensitive;
const int64_t max_line = min(virt_ext[i].max_line, line_limit);
goto_line(b, max_line - 1);
goto_pos(b, max_line == line_limit && pos_limit != -1 ? pos_limit : b->cur_line_desc->line_len);
b->find_string = virt_ext[i].regex;
b->find_string_changed = 1;
while (find_regexp(b, NULL, true, false) == OK) {
min_line = b->cur_line;
D(fprintf(stderr, "[%d] --- found match for '%s' on line <%d>\n", __LINE__, ext, min_line);)
if (min_line == 0) break;
}
if (min_line > -1) {
if (min_line < earliest_found_line) {
earliest_found_line = min_line;
ext = virt_ext[i].ext;
}
}
}
int word_wrap2(buffer * const b) {
static char avcmd[16];
if (b->cur_pos > b->cur_line_desc->line_len) return OK;
bool non_blank_added = false;
int avshift;
char * line = b->cur_line_desc->line;
int64_t pos, original_line;
/* If the char to our left is a space, we need to insert
a non-space to attach our WORDWRAP_BOOKMARK to because
spaces at the split point get removed, which effectively
leaves our bookmark on the current line. */
delay_update();
pos = prev_pos(line, b->cur_pos, b->encoding);
if (pos >= 0 && (non_blank_added = ne_isspace(get_char(&line[pos], b->encoding), b->encoding))) {
start_undo_chain(b);
insert_one_char(b, b->cur_line_desc, b->cur_line, b->cur_pos, 'X');
line = b->cur_line_desc->line;
goto_pos(b, next_pos(line, b->cur_pos, b->encoding));
}
b->bookmark[WORDWRAP_BOOKMARK].pos = b->cur_pos;
b->bookmark[WORDWRAP_BOOKMARK].line = original_line = b->cur_line;
b->bookmark[WORDWRAP_BOOKMARK].cur_y = b->cur_y;
b->bookmark_mask |= (1 << WORDWRAP_BOOKMARK);
paragraph(b);
goto_line_pos(b, b->bookmark[WORDWRAP_BOOKMARK].line, b->bookmark[WORDWRAP_BOOKMARK].pos);
line = b->cur_line_desc->line;
b->bookmark[WORDWRAP_BOOKMARK].cur_y += b->bookmark[WORDWRAP_BOOKMARK].line - original_line;
if (avshift = b->cur_y - b->bookmark[WORDWRAP_BOOKMARK].cur_y) {
snprintf(avcmd, 16, "%c%d", avshift > 0 ? 'T' :'B', avshift > 0 ? avshift : -avshift);
adjust_view(b, avcmd);
}
b->bookmark_mask &= ~(1 << WORDWRAP_BOOKMARK);
if (non_blank_added) {
goto_pos(b, prev_pos(b->cur_line_desc->line, b->cur_pos, b->encoding));
delete_one_char(b, b->cur_line_desc, b->cur_line, b->cur_pos);
end_undo_chain(b);
}
return stop ? STOPPED : OK;
}
static void
cmd_percent(key_info_t key_info, keys_info_t *keys_info)
{
int line;
if(key_info.count == NO_COUNT_GIVEN)
return;
if(key_info.count > 100)
return;
line = (key_info.count * view->list_rows)/100;
goto_pos(line - 1);
}
/* Scrolls pane by one view in both directions. The direction should be 1 or
* -1. */
static void
page_scroll(int base, int direction)
{
int new_pos;
/* Two lines gap. */
int lines = view->window_rows - 1;
int offset = lines*view->column_count;
new_pos = base + direction*offset;
scroll_by_files(view, direction*offset);
goto_pos(new_pos);
}
/* Scrolls pane by one view in both directions. The direction should be 1 or
* -1. */
static void
page_scroll(int base, int direction)
{
enum { HOR_GAP_SIZE = 2, VER_GAP_SIZE = 1 };
int offset = fview_is_transposed(view)
? MAX(1, (view->column_count - VER_GAP_SIZE))*view->window_rows
: (view->window_rows - HOR_GAP_SIZE)*view->column_count;
int new_pos = base + direction*offset
+ view->list_pos%view->run_size - base%view->run_size;
new_pos = MAX(0, MIN(view->list_rows - 1, new_pos));
scroll_by_files(view, direction*offset);
goto_pos(new_pos);
}
static void
cmd_ctrl_u(key_info_t key_info, keys_info_t *keys_info)
{
if(!at_first_line(view))
{
int new_pos;
size_t offset = view->window_cells/2;
offset = ROUND_DOWN(offset, view->column_count);
new_pos = get_corrected_list_pos_up(view, offset);
new_pos = MIN(new_pos, view->list_pos - (int)offset);
new_pos = MAX(new_pos, 0);
new_pos = ROUND_DOWN(new_pos, view->column_count);
view->top_line += new_pos - view->list_pos;
goto_pos(new_pos);
}
}
static void
cmd_ctrl_d(key_info_t key_info, keys_info_t *keys_info)
{
if(!at_last_line(view))
{
size_t new_pos;
size_t offset = view->window_cells/2;
offset = ROUND_DOWN(offset, view->column_count);
new_pos = get_corrected_list_pos_down(view, offset);
new_pos = MAX(new_pos, view->list_pos + offset);
new_pos = MIN(new_pos, (size_t)view->list_rows);
new_pos = ROUND_DOWN(new_pos, view->column_count);
view->top_line += new_pos - view->list_pos;
goto_pos(new_pos);
}
}
int replace(buffer * const b, const int n, const char * const string) {
int64_t len;
assert(string != NULL);
last_replace_empty_match = false;
len = strlen(string);
start_undo_chain(b);
delete_stream(b, b->cur_line_desc, b->cur_line, b->cur_pos, n);
if (len) insert_stream(b, b->cur_line_desc, b->cur_line, b->cur_pos, string, len);
end_undo_chain(b);
if (! b->opt.search_back) goto_pos(b, b->cur_pos + len);
return OK;
}
int find(buffer * const b, const char *pattern, const bool skip_first) {
bool recompile_string;
if (!pattern) {
pattern = b->find_string;
recompile_string = b->find_string_changed || b->last_was_regexp;
}
else recompile_string = true;
const int m = strlen(pattern);
if (!pattern || !m) return ERROR;
if (recompile_string) for(int i = 0; i < sizeof d / sizeof *d; i++) d[i] = m;
const unsigned char * const up_case = b->encoding == ENC_UTF8 ? ascii_up_case : localised_up_case;
const bool sense_case = (b->opt.case_search != 0);
line_desc *ld = b->cur_line_desc;
int64_t y = b->cur_line;
stop = false;
if (! b->opt.search_back) {
if (recompile_string) {
for(int i = 0; i < m - 1; i++) d[CONV((unsigned char)pattern[i])] = m - i-1;
b->find_string_changed = 0;
}
char * p = ld->line + b->cur_pos + m - 1 + (skip_first ? 1 : 0);
const unsigned char first_char = CONV((unsigned char)pattern[m - 1]);
while(y < b->num_lines && !stop) {
assert(ld->ld_node.next != NULL);
if (ld->line_len >= m) {
while((p - ld->line) < ld->line_len) {
const unsigned char c = CONV((unsigned char)*p);
if (c != first_char) p += d[c];
else {
int i;
for (i = 1; i < m; i++)
if (CONV((unsigned char)*(p - i)) != CONV((unsigned char)pattern[m - i-1])) {
p += d[c];
break;
}
if (i == m) {
goto_line(b, y);
goto_pos(b, (p - ld->line) - m + 1);
return OK;
}
}
}
}
ld = (line_desc *)ld->ld_node.next;
if (ld->ld_node.next) p = ld->line + m-1;
y++;
}
}
else {
if (recompile_string) {
for(int i = m - 1; i > 0; i--) d[CONV((unsigned char)pattern[i])] = i;
b->find_string_changed = 0;
}
char * p = ld->line + (b->cur_pos > ld->line_len - m ? ld->line_len - m : b->cur_pos + (skip_first ? -1 : 0));
const unsigned char first_char = CONV((unsigned char)pattern[0]);
while(y >= 0 && !stop) {
assert(ld->ld_node.prev != NULL);
if (ld->line_len >= m) {
while((p - ld->line) >= 0) {
const unsigned char c = CONV((unsigned char)*p);
if (c != first_char) p -= d[c];
else {
int i;
for (i = 1; i < m; i++)
if (CONV((unsigned char)*(p + i)) != CONV((unsigned char)pattern[i])) {
p -= d[c];
break;
}
if (i == m) {
goto_line(b, y);
goto_pos(b, p - ld->line);
return OK;
}
}
}
}
ld = (line_desc *)ld->ld_node.prev;
if (ld->ld_node.prev) p = ld->line + ld->line_len - m;
y--;
}
}
return stop ? STOPPED : NOT_FOUND;
}
int replace_regexp(buffer * const b, const char * const string) {
assert(string != NULL);
bool reg_used = false;
char *p, *q, *t = NULL;
if (q = p = str_dup(string)) {
int len = strlen(p);
while(true) {
while(*q && *q != '\\') q++;
if (!*q) break;
int i = *(q + 1) - '0';
if (*(q + 1) == '\\') {
memmove(q, q + 1, strlen(q + 1) + 1);
q++;
len--;
}
else if (i >= 0 && i < RE_NREGS && re_reg.start[i] >= 0) {
if (b->encoding == ENC_UTF8) {
/* In the UTF-8 case, the replacement group index must be
mapped through map_group to recover the real group. */
if ((i = map_group[i]) >= RE_NREGS) {
free(p);
return GROUP_NOT_AVAILABLE;
}
}
*q++ = 0;
*q++ = i;
reg_used = true;
}
else {
free(p);
return WRONG_CHAR_AFTER_BACKSLASH;
}
}
if (reg_used) {
if (t = malloc(re_reg.end[0] - re_reg.start[0] + 1)) {
memcpy(t, b->cur_line_desc->line + re_reg.start[0], re_reg.end[0] - re_reg.start[0]);
t[re_reg.end[0] - re_reg.start[0]] = 0;
}
else {
free(p);
return OUT_OF_MEMORY;
}
}
for(int i = re_reg.num_regs; i-- != 0;) {
re_reg.end[i] -= re_reg.start[0];
re_reg.start[i] -= re_reg.start[0];
}
start_undo_chain(b);
delete_stream(b, b->cur_line_desc, b->cur_line, b->cur_pos, re_reg.end[0]);
q = p;
int64_t pos = 0;
while(true) {
if (strlen(q)) {
insert_stream(b, b->cur_line_desc, b->cur_line, b->cur_pos + pos, q, strlen(q));
pos += strlen(q);
}
q += strlen(q) + 1;
if (q - p > len) break;
assert(*q < RE_NREGS);
if (re_reg.end[*(unsigned char *)q] - re_reg.start[*(unsigned char *)q]) {
char c = t[re_reg.end[*(unsigned char *)q]];
t[re_reg.end[*(unsigned char *)q]] = 0;
insert_stream(b, b->cur_line_desc, b->cur_line, b->cur_pos + pos, t + re_reg.start[*(unsigned char *)q], re_reg.end[*(unsigned char *)q] - re_reg.start[*(unsigned char *)q]);
t[re_reg.end[*(unsigned char *)q]] = c;
pos += re_reg.end[*(unsigned char *)q] - re_reg.start[*(unsigned char *)q];
}
q++;
}
end_undo_chain(b);
if (! b->opt.search_back) goto_pos(b, b->cur_pos + pos);
free(t);
free(p);
}
else return OUT_OF_MEMORY;
last_replace_empty_match = re_reg.start[0] == re_reg.end[0];
return OK;
}
int find_regexp(buffer * const b, const char *regex, const bool skip_first) {
const unsigned char * const up_case = b->encoding == ENC_UTF8 ? ascii_up_case : localised_up_case;
bool recompile_string;
if (!regex) {
regex = b->find_string;
recompile_string = b->find_string_changed || !b->last_was_regexp;
}
else recompile_string = true;
if (!regex || !strlen(regex)) return ERROR;
if (re_pb.buffer == NULL) {
if (re_pb.buffer = malloc(START_BUFFER_SIZE)) re_pb.allocated = START_BUFFER_SIZE;
else return OUT_OF_MEMORY;
}
re_pb.fastmap = (void *)d;
/* We have to be careful: even if the search string has not changed, it
is possible that case sensitivity has. In this case, we force recompilation. */
if (b->opt.case_search) {
if (re_pb.translate != 0) recompile_string = true;
re_pb.translate = 0;
}
else {
if (re_pb.translate != up_case) recompile_string = true;
re_pb.translate = (unsigned char *)up_case;
}
if (recompile_string) {
const char *actual_regex = regex;
/* If the buffer encoding is UTF-8, we need to replace dots with UTF8DOT,
non-word-constituents (\W) with UTF8NONWORD, and embed complemented
character classes in UTF8COMP, so that they do not match UTF-8
subsequences. Moreover, we must compute the remapping from the virtual
to the actual groups caused by the new groups thus introduced. */
if (b->encoding == ENC_UTF8) {
const char *s;
char *q;
bool escape = false;
int virtual_group = 0, real_group = 0, dots = 0, comps = 0, nonwords = 0;
s = regex;
/* We first scan regex to compute the exact number of characters of
the actual (i.e., after substitutions) regex. */
do {
if (!escape) {
if (*s == '.') dots++;
else if (*s == '[') {
if (*(s+1) == '^') {
comps++;
s++;
}
if (*(s+1) == ']') s++; /* A literal ]. */
/* We scan the list up to ] and check that no non-US-ASCII characters appear. */
do if (utf8len(*(++s)) != 1) return UTF8_REGEXP_CHARACTER_CLASS_NOT_SUPPORTED; while(*s && *s != ']');
}
else if (*s == '\\') {
escape = true;
continue;
}
}
else if (*s == 'W') nonwords++;
escape = false;
} while(*(++s));
actual_regex = q = malloc(strlen(regex) + 1 + (strlen(UTF8DOT) - 1) * dots + (strlen(UTF8NONWORD) - 2) * nonwords + (strlen(UTF8COMP) - 1) * comps);
if (!actual_regex) return OUT_OF_MEMORY;
s = regex;
escape = false;
do {
if (escape || *s != '.' && *s != '(' && *s != '[' && *s != '\\') {
if (escape && *s == 'W') {
q--;
strcpy(q, UTF8NONWORD);
q += strlen(UTF8NONWORD);
real_group++;
}
else *(q++) = *s;
}
else {
if (*s == '\\') {
escape = true;
*(q++) = '\\';
continue;
}
if (*s == '.') {
strcpy(q, UTF8DOT);
q += strlen(UTF8DOT);
real_group++;
}
else if (*s == '(') {
*(q++) = '(';
if (virtual_group < RE_NREGS - 1) map_group[++virtual_group] = ++real_group;
}
else if (*s == '[') {
if (*(s+1) == '^') {
strcpy(q, UTF8COMP);
q += strlen(UTF8COMP);
s++;
if (*(s+1) == ']') *(q++) = *(++s); /* A literal ]. */
do *(q++) = *(++s); while (*s && *s != ']');
if (*s) *(q++) = ')';
real_group++;
}
else {
*(q++) = '[';
if (*(s+1) == ']') *(q++) = *(++s); /* A literal ]. */
do *(q++) = *(++s); while (*s && *s != ']');
}
}
}
escape = false;
} while(*(s++));
/* This assert may be false if a [ is not closed. */
assert(strlen(actual_regex) == strlen(regex) + (strlen(UTF8DOT) - 1) * dots + (strlen(UTF8NONWORD) - 2) * nonwords + (strlen(UTF8COMP) - 1) * comps);
}
const char * p = re_compile_pattern(actual_regex, strlen(actual_regex), &re_pb);
if (b->encoding == ENC_UTF8) free((void*)actual_regex);
if (p) {
/* Here we have a very dirty hack: since we cannot return the error of
regex, we print it here. Which means that we access term.c's
functions. 8^( */
print_message(p);
alert();
return ERROR;
}
}
b->find_string_changed = 0;
line_desc *ld = b->cur_line_desc;
int64_t y = b->cur_line;
stop = false;
if (! b->opt.search_back) {
int64_t start_pos = b->cur_pos + (skip_first ? 1 : 0);
while(y < b->num_lines && !stop) {
assert(ld->ld_node.next != NULL);
int64_t pos;
if (start_pos <= ld->line_len &&
(pos = re_search(&re_pb, ld->line ? ld->line : "", ld->line_len, start_pos, ld->line_len - start_pos, &re_reg)) >= 0) {
goto_line(b, y);
goto_pos(b, pos);
return OK;
}
ld = (line_desc *)ld->ld_node.next;
start_pos = 0;
y++;
}
}
else {
int64_t start_pos = b->cur_pos + (skip_first ? -1 : 0);
while(y >= 0 && !stop) {
assert(ld->ld_node.prev != NULL);
int64_t pos;
if (start_pos >= 0 &&
(pos = re_search(&re_pb, ld->line ? ld->line : "", ld->line_len, start_pos, -start_pos - 1, &re_reg)) >= 0) {
goto_line(b, y);
goto_pos(b, pos);
return OK;
}
ld = (line_desc *)ld->ld_node.prev;
if (ld->ld_node.prev) start_pos = ld->line_len;
y--;
}
}
return stop ? STOPPED : NOT_FOUND;
}
void controlpanel_autonomy() {
float follow = 0;
char input = ' ';
OdomData odom;
while (true) {
char ch = controlpanel_promptChar("Autonomy");
switch (ch) {
case 'G': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
obstacleAvoidance_setEnabled(true);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'g': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'e':
if (goto_getEnabled()) {
printf_P(PSTR("Goto: enabled, "));
} else {
printf_P(PSTR("Goto: disabled, "));
}
if (magfollow_enabled()) {
printf_P(PSTR("Magfollow: enabled, "));
} else {
printf_P(PSTR("Magfollow: disabled, "));
}
if (obstacleAvoidance_getEnabled()) {
printf_P(PSTR("Obstacle Avoidance: enabled.\n"));
} else {
printf_P(PSTR("Obstacle Avoidance: disabled.\n"));
}
break;
case 's': { // Sets current heading of robot to prompted heading from user
float newheading;
controlpanel_prompt("Heading (deg)", "%f", &newheading);
magfollow_setHeading(degtorad(newheading));
break;
}
case 'h': { // Prints out magnetometer calibrated heading
float heading = magfollow_getHeading();
heading = radtodeg(heading);
printf_P(PSTR("Mag Heading (deg): %f\n"), heading);
break;
}
case 'w': {
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Follow at Heading (deg)", "%f", &follow);
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
break;
}
case 'a':
follow = follow + 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 'd':
follow = follow - 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 't':
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Turn to Heading (deg)", "%f", &follow);
follow = degtorad(follow);
printf_P(PSTR("Currently at (deg): %f, Turning to (deg): %f\n"), radtodeg(magfollow_getHeading()), radtodeg(follow));
magfollow_turn(setSpeed, anglewrap(follow));
break;
case 'o':
obstacleAvoidance_setEnabled(false);
printf_P(PSTR("Obstacle Avoidance Disabled.\n"));
break;
case 'O':
obstacleAvoidance_setEnabled(true);
printf_P(PSTR("Obstacle Avoidance Enabled!\n"));
break;
case 'c': {
printf_P(PSTR("Beginning auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
calibrate_stationary();
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'C': {
printf_P(PSTR("Beginning Competition Routine!\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
Field field = calibrate_competition();
float vel;
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
printf_P(PSTR("Running Spiral-In Course!\n"));
overlap_run(field.xi, field.yi, field.xj, field.yj, field.xk, field.yk, field.xl, field.yl, vel);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'i': {
printf_P(PSTR("Beginning competition auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
magfollow_setOffset(0);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
float xi, yi;
float xj, yj;
float xk, yk;
float xl, yl;
float vel;
controlpanel_prompt("Xi (meters): ", "%f", &xi);
controlpanel_prompt("Yi (meters): ", "%f", &yi);
controlpanel_prompt("Xj (meters): ", "%f", &xj);
controlpanel_prompt("Yj (meters): ", "%f", &yj);
controlpanel_prompt("Xk (meters): ", "%f", &xk);
controlpanel_prompt("Yk (meters): ", "%f", &yk);
controlpanel_prompt("Xl (meters): ", "%f", &xl);
controlpanel_prompt("Yl (meters): ", "%f", &yl);
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
spiralIn_run(xi, yi, xj, yj, xk, yk, xl, yl, vel);
break;
}
case 'f':
debug_halt("testing");
break;
case ' ':
magfollow_stop();
obstacleAvoidance_setEnabled(false);
goto_setEnabled(false);
break;
case 'q':
magfollow_stop();
return;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" G - Goto Coordinate w/ Obstacle Avoidance\n"
" g - Goto Coordinate\n"
" e - Print states of enables\n"
" s - Set Heading\n"
" h - Current Heading\n"
" w - Magfollow\n"
" a - Shift following left\n"
" d - Shift following right\n"
" t - MagTurn\n"
" O/o - Enable/Disable Obstacle Avoidance\n"
" c - Auto-Calibration Routine\n"
" C - Do Competition Routine\n"
" i - Run Spiral-In competition\n"
" f - Halt\n"
" ' ' - Stop\n"
" q - Quit";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
/* Move to the last line of window, selecting as we go. */
static void
cmd_L(key_info_t key_info, keys_info_t *keys_info)
{
size_t new_pos = get_window_bottom_pos(view);
goto_pos(new_pos);
}
/* Move to middle line of window, selecting from start position to there. */
static void
cmd_M(key_info_t key_info, keys_info_t *keys_info)
{
size_t new_pos = get_window_middle_pos(view);
goto_pos(new_pos);
}
void view(void)
{
static byte mod;
static unsigned k;
static unsigned long delta;
static byte i;
load_headers();
in_buf=0;
ateof=0;
atstart=1;
scrollbar_pos=0xa000+29;
if(last==0) last=header_length;
goto_pos(last);
mod=1;
ozcls();
while(1)
{
if(mod)
{
if(!backandforth)position_save();
show();
unbar();
scrollbar();
backandforth=mod=0;
}
switch(k=ozgetchblank())
{
case KEY_LEFT:
case KEY_BACKSPACE:
#ifdef DEBUG
putformatted(0,0,"positionptr=| numpositions=| ",
positionptr,numpositions);
ozgetch();
#endif
if(positionptr<=1) break;
goto_pos(positions[(--positionptr)-1]);
if(!positionptr) positionptr=1;
backandforth=mod=1;
#ifdef DEBUG
putformatted(0,0,"positionptr=| numpositions=| ",
positionptr,numpositions);
ozgetch();
#endif
break;
case KEY_RIGHT:
#ifdef DEBUG
putformatted(0,0,"positionptr=| numpositions=| ",
positionptr,numpositions);
ozgetch();
#endif
if(positionptr>=numpositions) break;
goto_pos(positions[positionptr]);
positionptr++;
backandforth=mod=1;
#ifdef DEBUG
putformatted(0,0,"positionptr=| numpositions=| ",
positionptr,numpositions);
ozgetch();
#endif
break;
case 'm':
/* mark position */
if(numbookmarks>=MAX_BOOKMARKS)
{
for(i=0;i<MAX_BOOKMARKS-1;i++)
{
bookmarks[i]=bookmarks[i+1];
}
numbookmarks--;
}
bookmarkptr=numbookmarks;
bookmarks[numbookmarks++]=screen_offset+buffer_offset;
bookmarksmod=1;
break;
case 'b':
if(numbookmarks)
{
goto_pos(bookmarks[bookmarkptr]);
if(bookmarkptr) bookmarkptr--;
else bookmarkptr=numbookmarks-1;
mod=1;
}
break;
case 'r':
show_bar=!show_bar;
bookmarksmod=1;
unbar();
scrollbar();
break;
case 'h':
case KEY_UPPER_MENU:
case KEY_LOWER_MENU:
dohelp();
ozcls();
mod=1;
break;
case 'i':
_ozfilledbox(0,0,WIDTH,line_height,0);
delta=screen_offset+buffer_offset-header_length;
putformatted(0,0,"$ : ^/^=|%",filename,
delta,length-header_length,
(unsigned int)(delta*100/(length-header_length)) );
_ozfilledbox(0,0,WIDTH,line_height,XOR);
ozgetchblank();
mod=1;
ozcls();
break;
case 'c':
if(numbookmarks && ozwarn("Delete all bookmarks?",yn)==KEY_LOWER_ENTER
)
{
bookmarksmod=1;
numbookmarks=0;
}
mod=1;
ozcls();
break;
case '-':
case KEY_PAGEUP:
for(i=0;i<num_lines-1;i++)
{
checkshiftup();
backline();
}
mod=1;
break;
case 's':
if(myfont) break;
line_height++;
if(line_height==11) line_height=8;
if(line_height!=9) num_lines=80/line_height;
else num_lines=9;
bookmarksmod=1;
mod=1;
ozcls();
break;
case KEY_PAGEDOWN:
case KEY_LOWER_ENTER:
case KEY_UPPER_ENTER:
case KEY_RETURN:
case ' ':
for(i=0;i<num_lines-1;i++)
{
checkshiftdown();
forwardline();
}
mod=1;
break;
case KEY_UP:
checkshiftup();
if(!backline()) break;
unbar();
ozscrolldown(line_height*30);
dcompline(filebuf+screen_offset);
puttextline(0);
position_save();
scrollbar();
break;
case KEY_DOWN:
checkshiftdown();
if(forwardline())
{
mod=1;
showbot=1;
unbar();
ozscroll(line_height*30);
}
break;
case KEY_LEFT_SHIFT | MASKSHIFT:
case KEY_RIGHT_SHIFT | MASKSHIFT:
switch(ozgetch() & 0xF0FF)
{
case '-':
ozclick(!ozgetclick());
ozsavekeysettings();
break;
case KEY_PAGEUP:
case KEY_UP:
tenpercent(-1);
mod=1;
break;
case KEY_PAGEDOWN:
case KEY_DOWN:
tenpercent(1);
mod=1;
break;
}
break;
case KEY_UP | MASKSHIFT:
case KEY_PAGEUP | MASKSHIFT:
tenpercent(-1);
mod=1;
break;
case KEY_PAGEDOWN | MASKSHIFT:
case KEY_DOWN | MASKSHIFT:
tenpercent(1);
mod=1;
break;
case KEY_2ND:
switch(ozgetch() & 0xF0FF )
{
case '-':
ozclick(!ozgetclick());
ozsavekeysettings();
break;
case KEY_PAGEUP:
case KEY_UP:
goto_pos(header_length);
mod=1;
break;
case KEY_PAGEDOWN:
case KEY_DOWN:
goto_pos(length);
mod=1;
break;
}
break;
case KEY_LOWER_ESC:
case KEY_UPPER_ESC:
case KEY_MYPROGRAMS:
exit(0);
case KEY_MAIN:
case KEY_CALENDAR:
case KEY_TELEPHONE:
case KEY_MEMO:
ozexitto(k);
case KEY_BACKLIGHT:
oztogglelight();
break;
}
}
}
/* Move to middle line of the window, selecting from start position to there. */
static void
cmd_M(key_info_t key_info, keys_info_t *keys_info)
{
goto_pos(fpos_get_middle_pos(view));
}
/* Move to the last line of window, selecting as we go. */
static void
cmd_L(key_info_t key_info, keys_info_t *keys_info)
{
goto_pos(fpos_get_bottom_pos(view));
}
/* Move to the first line of window, selecting as we go. */
static void
cmd_H(key_info_t key_info, keys_info_t *keys_info)
{
goto_pos(fpos_get_top_pos(view));
}