task main()
{
// Going forward at full speed
go_forward(100);
nxtDisplayCenteredTextLine (3, "Going Forward");
wait1Msec (Drive_Time);
// Going left at half speed
go_left(50);
nxtDisplayCenteredTextLine (3, "Turning Left");
wait1Msec (Turn_Time);
go_forward(100);
nxtDisplayCenteredTextLine (3, "Going Forward");
wait1Msec (Drive_Time);
go_left(50);
nxtDisplayCenteredTextLine (3, "Turning Left");
wait1Msec (Turn_Time);
go_forward(100);
nxtDisplayCenteredTextLine (3, "Going Forward");
wait1Msec (Drive_Time);
go_left(50);
nxtDisplayCenteredTextLine (3, "Turning Left");
wait1Msec (Turn_Time);
go_forward(100);
nxtDisplayCenteredTextLine (3, "Going Forward");
wait1Msec (Drive_Time);
go_left(50);
nxtDisplayCenteredTextLine (3, "Turning Left");
wait1Msec (Turn_Time);
StopAllTasks ();
}
static int get_total_len(t_data *data)
{
int i;
int n;
n = 0;
i = 0;
while (data->line[i])
{
if (is_quote(data->line[i]))
i = go_forward(data->line, i, &n);
if (ft_isspace(data->line[i]))
{
i++;
n++;
while (data->line[i] && ft_isspace(data->line[i]))
i++;
}
else
{
i++;
n++;
}
}
return (n);
}
void loop() {
unsigned int cm = DistanceSensor.ping_cm();
Serial.print("Distance: ");
Serial.print(cm);
Serial.println("cm");
delay(scan_time);
if(cm < 5){
spin_random();
scan_time = NORMAL_SCAN_TIME;
}else if (cm < 20) {
stop();
spin_random();
scan_time = FAST_SCAN_TIME;
} else if(cm == 30){
// Serial.print("DANS!");
// stop();
// delay(1000);
// spin_left();
// delay(3000);
// spin_right();
// delay(3000);
// stop();
}else {
go_forward();
scan_time = FAST_SCAN_TIME;
}
}
void loop() {
unsigned long distance = sense_distance();
if (distance >= DISTANCE_THRESHOLD) {
go_forward();
} else {
go_backward();
turn_left();
}
}
//==============================
//==============================
int main(int argc, char *argv[])
{
int ret;
//printf("[wifibot - go]\n");
/*
if(argc==4)
{
strcpy(host_addr,argv[1]);
target_distance=100*atof(argv[2]); //convert to cm
target_angle=atof(argv[3]);
}
else
{
printf("syntax: go [address] [distance] [angle]\n");
exit(1);
}
*/
if(process_args(argc, argv)<0)
{
printf("Syntax: move -h <address> -d <distance> -a <angle> -s <speed>\n");
exit(1);
}
if((socket_command=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0)
{
fprintf(stderr,"Command socket error\n");
exit(1);
}
memset((char *) &myaddr_command, 0, sizeof(myaddr_command));
myaddr_command.sin_family=AF_INET;
myaddr_command.sin_port=htons(PORT_COMMAND);
myaddr_command.sin_addr.s_addr=inet_addr(host_addr); //inet_addr(HOST_ADDR);
stop_motors();
if((ret = pthread_create(&threads[0], NULL, th_robot_status, (void *) 0)) != 0)
{
fprintf(stderr,"Status thread (%s)\n", strerror(ret));
exit (1);
}
/*
if((ret = pthread_create(&threads[1], NULL, th_gps, (void *) 0)) != 0)
{
printf("ERRR gps thread (%s)\n", strerror(ret));
exit (1);
}
*/
while(!wait_for_status)
{
usleep(50000);
}
// do the thing and exit
//printf("DIST_START: %.2f\n",target_distance);
me.dist_from_start=0;
rotate();
go_forward();
printf("[wifibot] %.2f\n",me.dist_from_start/100);
return 0;
}
/*Ce code marche car au moment de l'insertin it et balle_inseree sont au meme point courant*/
void avancer_balls(node_ball* it,list_ball* l)
{
int i=0;
while(it!=l->head )
{
for(i=0;i<32;++i)
{
go_forward(&(it->val));
}
it=it->previous;
}
}
void main()
{
setup_timer_2(T2_DIV_BY_16,255,1); //4.0 ms overflow, 4.0 ms interrupt
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
set_pwm1_duty((int16)200);
set_pwm2_duty((int16)200);
enable_interrupts(INT_SSP);
enable_interrupts(GLOBAL);
SET_TRIS_B(0);
stop();
//delay_ms(500);
while(TRUE)
{
if(hasCommand)
{
hasCommand= FALSE;
if(buffer[0] == 's'){
show();
}
else
switch(buffer[1])
{
case 1:
go_forward();
break;
case 2:
go_backward();
break;
case 3:
turn_right();
break;
case 4:
turn_left();
break;
case 5:
stop();
break;
default :
break;
}
}
}
}
void segue_coordenadas(int8 dados)
{
//o objetivo é centralizar a bolinha antes de seguir em frente
int16 obstaculo = 0;
switch (dados) {
case CEL_DIREITA:
turn_right();
#ifdef DEBUG
printf("cellphone turning right...\n");
#endif
break;
case CEL_ESQUERDA:
turn_left();
#ifdef DEBUG
printf("cellphone turning left...\n");
#endif
break;
case CEL_FRENTE:
#ifdef sensorir
obstaculo = verificaObstaculo();
if (obstaculo == 0) {
#endif
go_forward();
#ifdef DEBUG
printf("no obstacle, go ahead...\n");
#endif
#ifdef sensorir
} else if (obstaculo == 1) {
go_idle();
obstaculo = 0;
#ifdef DEBUG
printf("obstacle, going idle to not crash...\n");
#endif
}
#endif
default:
break;
}
}
gui::slider::slider(int p_x, int p_y, unsigned int p_length, int p_items, int p_space, orientation p_orientation) {
x = p_x;
y = p_y;
m_orientation = p_orientation;
width = (m_orientation == orientation::horizontal? p_length : 24);
height = (m_orientation == orientation::vertical? p_length : 24);
slider_box.setFillColor(colors::charcoal);
slider_box.setOutlineColor(colors::black);
slider_box.setOutlineThickness(1.0f);
items = p_items;
space = p_space;
if (m_orientation == orientation::horizontal) {
const sf::Texture& right_arrow = resource_cache::get_texture(resource_directory::get("slider_arrow_right"));
const sf::Texture& left_arrow = resource_cache::get_texture(resource_directory::get("slider_arrow_left"));
slider_pre_arrow = make_unique<texture_button>(left_arrow);
slider_pre_arrow = make_unique<texture_button>(left_arrow);
slider_post_arrow = make_unique<texture_button>(right_arrow);
slider_button = make_unique<generic_button>(0, 0, 0, 0, colors::matterhorn);
} else if (m_orientation == orientation::vertical) {
const sf::Texture& up_arrow = resource_cache::get_texture(resource_directory::get("slider_arrow_up"));
const sf::Texture& down_arrow = resource_cache::get_texture(resource_directory::get("slider_arrow_down"));
slider_pre_arrow = make_unique<texture_button>(up_arrow);
slider_post_arrow = make_unique<texture_button>(down_arrow);
slider_button = make_unique<generic_button>(0, 0, 0, 0, colors::matterhorn);
}
slider_pre_arrow->set_callback([this] { go_backward(); });
slider_post_arrow->set_callback([this] { go_forward(); });
minimum = 0;
maximum = items >= space? items - space : 0;
magnitude = 0;
update_dimensions();
}
FUNCTION objend ()
{
register Ocb * o;
extern int aggressive;
Debug
o = xqting_ocb;
destroy_message_vector (o);
if ( o->centry == DCRT || o->centry == DDES )
{
unmark_macro ( o->ci );
}
else
{
register Msgh * n;
for ( n = fstigb ( o->ci ); n; n = nxtigb ( n ) )
{
if ( n->mtype != DYNDSMSG )
unmark_macro ( n );
}
}
if ( o->centry == INIT || o->centry == DCRT || o->centry == DDES )
{
#ifdef SOM
/* Update the phase's highest-seen ept field to the Ept of the event
just completed. Also update the amount of total unrolled back work
done by the phase. */
o->Ept = o->sb->Ept;
#endif SOM
save_state (o);
}
if ( o->centry == EVENT )
{
o->stats.numecomp++;
o->eventTimePermitted -= o->sb->effectWork;
#ifdef SOM
/* Update the phase's highest-seen ept field to the Ept of the event
just completed. Also update the amount of total unrolled back work
done by the phase. */
o->Ept = o->sb->Ept;
#endif SOM
save_state ( o );
}
else
if ( o->centry == TERM )
{
#ifdef RBC
if ( o->uses_rbc )
l_destroy ( o->sb );
else
/* destroy_state and rollback chip don't mix */
#endif
destroy_state ( o->sb );
o->sb = NULL;
l_destroy ( o->stk );
o->stk = NULL;
#ifdef RBC
if ( o->uses_rbc && rollback_op ( o, 1, posinfPlus1 ) )
{
printf ( "weird error term objend for %s\n", o->name );
tester();
}
#endif
o->ci = NULL;
o->co = NULL;
o->control = EDGE;
o->runstat = BLKINF;
if ( ! aggressive )
cancel_omsgs ( o, o->svt, o->phase_end );
l_remove ( o );
o->svt = posinfPlus1;
l_insert ( l_prev_macro ( _prqhd ), o );
dispatch ();
return;
}
go_forward ( o ) ;
dispatch ();
}
void do_normalmode(struct block * buf) {
int bs = get_bufsize(buf);
struct ent * e;
switch (buf->value) {
// Movement commands
case 'j':
case OKEY_DOWN:
currow = forw_row(1)->row;
unselect_ranges();
update(TRUE);
break;
case 'k':
case OKEY_UP:
currow = back_row(1)->row;
unselect_ranges();
update(TRUE);
break;
case 'h':
case OKEY_LEFT:
curcol = back_col(1)->col;
unselect_ranges();
update(TRUE);
break;
case 'l':
case OKEY_RIGHT:
curcol = forw_col(1)->col;
unselect_ranges();
update(TRUE);
break;
case '0':
case OKEY_HOME:
curcol = left_limit()->col;
unselect_ranges();
update(TRUE);
break;
case '$':
case OKEY_END:
curcol = right_limit()->col;
unselect_ranges();
update(TRUE);
break;
case '^':
currow = goto_top()->row;
unselect_ranges();
update(TRUE);
break;
case '#':
currow = goto_bottom()->row;
unselect_ranges();
update(TRUE);
break;
// Tick
case '\'':
if (bs != 2) break;
unselect_ranges();
e = tick(buf->pnext->value);
if (row_hidden[e->row]) {
scerror("Cell row is hidden");
break;
}
if (col_hidden[e->col]) {
scerror("Cell column is hidden");
break;
}
currow = e->row;
curcol = e->col;
update(TRUE);
break;
// CTRL j
case ctl('j'):
{
int p, c = curcol, cf = curcol;
if ( (p = is_range_selected()) != -1) {
struct srange * sr = get_range_by_pos(p);
c = sr->tlcol;
cf = sr->brcol;
}
auto_justify(c, cf, DEFWIDTH); // auto justificado de columnas
update(TRUE);
break;
}
// CTRL d
case ctl('d'): // set date format using current locate D_FMT format
{
#ifdef USELOCALE
#include <locale.h>
#include <langinfo.h>
char * loc = NULL;
char * f = NULL;
loc = setlocale(LC_TIME, "");
if (loc != NULL) {
f = nl_langinfo(D_FMT);
} else {
scerror("No locale set. Nothing changed");
}
int p, r = currow, c = curcol, rf = currow, cf = curcol;
if ( (p = is_range_selected()) != -1) {
struct srange * sr = get_range_by_pos(p);
r = sr->tlrow;
c = sr->tlcol;
rf = sr->brrow;
cf = sr->brcol;
}
if (any_locked_cells(r, c, rf, cf)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
dateformat(lookat(r, c), lookat(rf, cf), f);
update(TRUE);
break;
#else
scinfo("Build made without USELOCALE enabled");
#endif
}
// CTRL f
case ctl('f'):
case OKEY_PGDOWN:
{
int n = LINES - RESROW - 1;
if (atoi(get_conf_value("half_page_scroll"))) n = n / 2;
struct ent * e = forw_row(n);
currow = e->row;
unselect_ranges();
scroll_down(n);
update(TRUE);
break;
}
// CTRL b
case ctl('b'):
case OKEY_PGUP:
{
int n = LINES - RESROW - 1;
if (atoi(get_conf_value("half_page_scroll"))) n = n / 2;
currow = back_row(n)->row;
unselect_ranges();
scroll_up(n);
update(TRUE);
break;
}
case 'w':
e = go_forward();
currow = e->row;
curcol = e->col;
unselect_ranges();
update(TRUE);
break;
case 'b':
e = go_backward();
currow = e->row;
curcol = e->col;
unselect_ranges();
update(TRUE);
break;
case '/':
{
char cadena[] = ":int goto ";
int i;
for (i=0; i<strlen(cadena); i++) {
flush_buf(buf);
addto_buf(buf, cadena[i]);
exec_single_cmd(buf);
}
break;
}
case 'H':
currow = vert_top()->row;
unselect_ranges();
update(TRUE);
break;
case 'M':
currow = vert_middle()->row;
unselect_ranges();
update(TRUE);
break;
case 'L':
currow = vert_bottom()->row;
unselect_ranges();
update(TRUE);
break;
case 'G': // goto end
e = go_end();
currow = e->row;
curcol = e->col;
unselect_ranges();
update(TRUE);
break;
// GOTO goto
case ctl('a'):
e = go_home();
curcol = e->col;
currow = e->row;
unselect_ranges();
update(TRUE);
break;
case 'g':
if (buf->pnext->value == '0') { // g0
curcol = go_bol()->col;
} else if (buf->pnext->value == '$') { // g$
curcol = go_eol()->col;
} else if (buf->pnext->value == 'g') { // gg
e = go_home();
curcol = e->col;
currow = e->row;
} else if (buf->pnext->value == 'G') { // gG
e = go_end();
currow = e->row;
curcol = e->col;
} else if (buf->pnext->value == 'M') { // gM
curcol = horiz_middle()->col;
} else { // gA4 (goto cell)
(void) sprintf(interp_line, "goto %s", parse_cell_name(1, buf));
send_to_interp(interp_line);
}
unselect_ranges();
update(TRUE);
break;
// repeat last command
case '.':
copybuffer(lastcmd_buffer, buf); // nose graba en lastcmd_buffer!!
cmd_multiplier = 1;
exec_mult(buf, COMPLETECMDTIMEOUT);
break;
// enter command mode
case ':':
clr_header(input_win, 0);
chg_mode(':');
#ifdef HISTORY_FILE
add(commandline_history, "");
#endif
print_mode(input_win);
wrefresh(input_win);
handle_cursor();
inputline_pos = 0;
break;
// enter visual mode
case 'v':
chg_mode('v');
handle_cursor();
clr_header(input_win, 0);
print_mode(input_win);
wrefresh(input_win);
start_visualmode(currow, curcol, currow, curcol);
break;
// INPUT COMMANDS
case '=':
case '\\':
case '<':
case '>':
if (locked_cell(currow, curcol)) return;
insert_edit_submode = buf->value;
chg_mode(insert_edit_submode);
clr_header(input_win, 0);
print_mode(input_win);
wrefresh(input_win);
inputline_pos = 0;
break;
// EDITION COMMANDS
// edit cell (v)
case 'e':
if (locked_cell(currow, curcol)) return;
clr_header(input_win, 0);
inputline_pos = 0;
if (start_edit_mode(buf, 'v')) show_header(input_win);
break;
// edit cell (s)
case 'E':
if (locked_cell(currow, curcol)) return;
clr_header(input_win, 0);
inputline_pos = 0;
if (start_edit_mode(buf, 's')) show_header(input_win);
else {
scinfo("No string value to edit");
chg_mode('.');
show_celldetails(input_win);
print_mode(input_win);
wrefresh(input_win);
}
break;
// del current cell or range
case 'x':
del_selected_cells();
update(TRUE);
break;
// format col
case 'f':
if (bs != 2) return;
formatcol(buf->pnext->value);
break;
// mark cell or range
case 'm':
if (bs != 2) break;
int p = is_range_selected();
if (p != -1) { // mark range
struct srange * sr = get_range_by_pos(p);
set_range_mark(buf->pnext->value, sr);
} else // mark cell
set_cell_mark(buf->pnext->value, currow, curcol);
modflg++;
break;
// copy
case 'c':
{
if (bs != 2) break;
struct mark * m = get_mark(buf->pnext->value);
if ( m == NULL) return;
// if m represents a range
if ( m->row == -1 && m->col == -1) {
srange * r = m->rng;
yank_area(r->tlrow, r->tlcol, r->brrow, r->brcol, 'a', cmd_multiplier);
if (paste_yanked_ents(0, 'c') == -1) {
scerror("Locked cells encountered. Nothing changed");
break;
}
// if m represents just one cell
} else {
struct ent * p = *ATBL(tbl, get_mark(buf->pnext->value)->row, get_mark(buf->pnext->value)->col);
struct ent * n;
int c1;
#ifdef UNDO
create_undo_action();
#endif
for (c1 = curcol; cmd_multiplier-- && c1 < maxcols; c1++) {
if ((n = * ATBL(tbl, currow, c1))) {
if (n->flags & is_locked)
continue;
if (! p) {
clearent(n);
continue;
}
} else {
if (! p) break;
n = lookat(currow, c1);
}
#ifdef UNDO
copy_to_undostruct(currow, c1, currow, c1, 'd');
#endif
copyent(n, p, currow - get_mark(buf->pnext->value)->row, c1 - get_mark(buf->pnext->value)->col, 0, 0, maxrow, maxcol, 0);
n->row += currow - get_mark(buf->pnext->value)->row;
n->col += c1 - get_mark(buf->pnext->value)->col;
n->flags |= is_changed;
#ifdef UNDO
copy_to_undostruct(currow, c1, currow, c1, 'a');
#endif
}
#ifdef UNDO
end_undo_action();
#endif
}
if (atoi(get_conf_value("autocalc"))) EvalAll();
update(TRUE);
break;
}
// repeat last goto command
case 'n':
go_last();
update(TRUE);
break;
// range lock / unlock / valueize
case 'r':
{
int p, r = currow, c = curcol, rf = currow, cf = curcol;
if ( (p = is_range_selected()) != -1) {
struct srange * sr = get_range_by_pos(p);
r = sr->tlrow;
c = sr->tlcol;
rf = sr->brrow;
cf = sr->brcol;
}
if (buf->pnext->value == 'l') {
lock_cells(lookat(r, c), lookat(rf, cf));
} else if (buf->pnext->value == 'u') {
unlock_cells(lookat(r, c), lookat(rf, cf));
} else if (buf->pnext->value == 'v') {
valueize_area(r, c, rf, cf);
}
update(TRUE);
break;
}
// create range with two marks
case 'R':
if (bs == 3) {
create_range(buf->pnext->value, buf->pnext->pnext->value, NULL, NULL);
update(TRUE);
}
break;
// Zr Zc - Zap col or row - Show col or row - Sr Sc
case 'Z':
case 'S':
{
int rs, r = currow, c = curcol, arg = cmd_multiplier;
struct srange * sr;
if ( (rs = is_range_selected()) != -1) {
sr = get_range_by_pos(rs);
cmd_multiplier = 1;
r = sr->tlrow;
c = sr->tlcol;
arg = buf->pnext->value == 'r' ? sr->brrow - sr->tlrow + 1 : sr->brcol - sr->tlcol + 1;
}
if (buf->value == 'Z' && buf->pnext->value == 'r') {
hide_row(r, arg);
} else if (buf->value == 'Z' && buf->pnext->value == 'c') {
hide_col(c, arg);
} else if (buf->value == 'S' && buf->pnext->value == 'r') {
show_row(r, arg);
} else if (buf->value == 'S' && buf->pnext->value == 'c') {
show_col(c, arg);
}
cmd_multiplier = 0;
update(TRUE);
break;
}
// shift range or cell
case 's':
{
int p, r = currow, c = curcol, rf = currow, cf = curcol;
if ( (p = is_range_selected()) != -1) {
struct srange * sr = get_range_by_pos(p);
r = sr->tlrow;
c = sr->tlcol;
rf = sr->brrow;
cf = sr->brcol;
}
if ( any_locked_cells(r, c, rf, cf) && (buf->pnext->value == 'h' || buf->pnext->value == 'k') ) {
scerror("Locked cells encountered. Nothing changed");
return;
}
#ifdef UNDO
create_undo_action();
#endif
int ic = cmd_multiplier + 1;
switch (buf->pnext->value) {
case 'j':
fix_marks( (rf - r + 1) * cmd_multiplier, 0, r, maxrow, c, cf);
#ifdef UNDO
save_undo_range_shift(cmd_multiplier, 0, r, c, rf + (rf-r+1) * (cmd_multiplier - 1), cf);
#endif
while (ic--) shift_range(ic, 0, r, c, rf, cf);
break;
case 'k':
fix_marks( -(rf - r + 1) * cmd_multiplier, 0, r, maxrow, c, cf);
yank_area(r, c, rf + (rf-r+1) * (cmd_multiplier - 1), cf, 'a', cmd_multiplier); // keep ents in yanklist for sk
#ifdef UNDO
copy_to_undostruct(r, c, rf + (rf-r+1) * (cmd_multiplier - 1), cf, 'd');
save_undo_range_shift(-cmd_multiplier, 0, r, c, rf + (rf-r+1) * (cmd_multiplier - 1), cf);
#endif
while (ic--) shift_range(-ic, 0, r, c, rf, cf);
#ifdef UNDO
copy_to_undostruct(r, c, rf + (rf-r+1) * (cmd_multiplier - 1), cf, 'a');
#endif
break;
case 'h':
fix_marks(0, -(cf - c + 1) * cmd_multiplier, r, rf, c, maxcol);
yank_area(r, c, rf, cf + (cf-c+1) * (cmd_multiplier - 1), 'a', cmd_multiplier); // keep ents in yanklist for sk
#ifdef UNDO
copy_to_undostruct(r, c, rf, cf + (cf-c+1) * (cmd_multiplier - 1), 'd');
save_undo_range_shift(0, -cmd_multiplier, r, c, rf, cf + (cf-c+1) * (cmd_multiplier - 1));
#endif
while (ic--) shift_range(0, -ic, r, c, rf, cf);
#ifdef UNDO
copy_to_undostruct(r, c, rf, cf + (cf-c+1) * (cmd_multiplier - 1), 'a');
#endif
break;
case 'l':
fix_marks(0, (cf - c + 1) * cmd_multiplier, r, rf, c, maxcol);
#ifdef UNDO
save_undo_range_shift(0, cmd_multiplier, r, c, rf, cf + (cf-c+1) * (cmd_multiplier - 1));
#endif
while (ic--) shift_range(0, ic, r, c, rf, cf);
break;
}
#ifdef UNDO
end_undo_action();
#endif
cmd_multiplier = 0;
unselect_ranges();
update(TRUE);
break;
}
// delete row or column, or selected cell or range
case 'd':
{
if (bs != 2) return;
int ic = cmd_multiplier; // orig
if (buf->pnext->value == 'r') {
if (any_locked_cells(currow, 0, currow + cmd_multiplier, maxcol)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
#ifdef UNDO
create_undo_action();
copy_to_undostruct(currow, 0, currow + ic - 1, maxcol, 'd');
save_undo_range_shift(-ic, 0, currow, 0, currow - 1 + ic, maxcol);
#endif
fix_marks(-ic, 0, currow + ic - 1, maxrow, 0, maxcol);
yank_area(currow, 0, currow - 1 + cmd_multiplier, maxcol, 'r', ic);
while (ic--) deleterow();
#ifdef UNDO
copy_to_undostruct(currow, 0, currow - 1 + cmd_multiplier, maxcol, 'a');
end_undo_action();
#endif
if (cmd_multiplier > 0) cmd_multiplier = 0;
} else if (buf->pnext->value == 'c') {
if (any_locked_cells(0, curcol, maxrow, curcol + cmd_multiplier)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
#ifdef UNDO
create_undo_action();
copy_to_undostruct(0, curcol, maxrow, curcol - 1 + ic, 'd');
save_undo_range_shift(0, -ic, 0, curcol, maxrow, curcol - 1 + ic);
#endif
fix_marks(0, -ic, 0, maxrow, curcol - 1 + ic, maxcol);
yank_area(0, curcol, maxrow, curcol + cmd_multiplier - 1, 'c', ic);
while (ic--) deletecol();
#ifdef UNDO
copy_to_undostruct(0, curcol, maxrow, curcol + cmd_multiplier - 1, 'a');
end_undo_action();
#endif
if (cmd_multiplier > 0) cmd_multiplier = 0;
} else if (buf->pnext->value == 'd') {
del_selected_cells();
}
update(TRUE);
break;
}
// insert row or column
case 'i':
{
if (bs != 2) return;
#ifdef UNDO
create_undo_action();
#endif
if (buf->pnext->value == 'r') {
#ifdef UNDO
save_undo_range_shift(1, 0, currow, 0, currow, maxcol);
#endif
fix_marks(1, 0, currow, maxrow, 0, maxcol);
insert_row(0);
} else if (buf->pnext->value == 'c') {
#ifdef UNDO
save_undo_range_shift(0, 1, 0, curcol, maxrow, curcol);
#endif
fix_marks(0, 1, 0, maxrow, curcol, maxcol);
insert_col(0);
}
#ifdef UNDO
end_undo_action();
#endif
update(TRUE);
break;
}
case 'y':
// yank row
if ( bs == 2 && buf->pnext->value == 'r') {
yank_area(currow, 0, currow + cmd_multiplier - 1, maxcol, 'r', cmd_multiplier);
if (cmd_multiplier > 0) cmd_multiplier = 0;
// yank col
} else if ( bs == 2 && buf->pnext->value == 'c') {
yank_area(0, curcol, maxrow, curcol + cmd_multiplier - 1, 'c', cmd_multiplier);
if (cmd_multiplier > 0) cmd_multiplier = 0;
// yank cell
} else if ( bs == 2 && buf->pnext->value == 'y' && is_range_selected() == -1) {
yank_area(currow, curcol, currow, curcol, 'e', cmd_multiplier);
// yank range
} else if ( bs == 1 && is_range_selected() != -1) {
srange * r = get_selected_range();
yank_area(r->tlrow, r->tlcol, r->brrow, r->brcol, 'a', cmd_multiplier);
}
break;
// paste cell below or left
case 'p':
if (paste_yanked_ents(0, 'a') == -1) {
scerror("Locked cells encountered. Nothing changed");
break;
}
update(TRUE);
break;
case 'P':
case 'T':
if (bs != 2) break;
if (buf->pnext->value == 'v' || buf->pnext->value == 'f' || buf->pnext->value == 'c') {
int res = buf->value == 'P' ? paste_yanked_ents(0, buf->pnext->value) : paste_yanked_ents(1, buf->pnext->value); // paste cell above or right
if (res == -1) {
scerror("Locked cells encountered. Nothing changed");
break;
}
update(TRUE);
}
break;
// paste cell above or right
case 't':
if (paste_yanked_ents(1, 'a') == -1) {
scerror("Locked cells encountered. Nothing changed");
break;
}
update(TRUE);
break;
// select inner range - Vir
case 'V':
if (buf->value == 'V' && bs == 3 &&
buf->pnext->value == 'i' && buf->pnext->pnext->value == 'r') {
int tlrow = currow;
int brrow = currow;
int tlcol = curcol;
int brcol = curcol;
int * tlr = &tlrow;
int * brr = &brrow;
int * tlc = &tlcol;
int * brc = &brcol;
select_inner_range(tlr, tlc, brr, brc);
start_visualmode(*tlr, *tlc, *brr, *brc);
}
break;
// autojus
case 'a':
if ( bs != 2 ) break;
if (buf->pnext->value == 'a') {
int p, r = currow, c = curcol, rf = currow, cf = curcol;
if ( (p = is_range_selected()) != -1) {
struct srange * sr = get_range_by_pos(p);
r = sr->tlrow;
c = sr->tlcol;
rf = sr->brrow;
cf = sr->brcol;
}
if (any_locked_cells(r, c, rf, cf)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
char cline [BUFFERSIZE];
sprintf(cline, "autojus %s:", coltoa(c));
sprintf(cline + strlen(cline), "%s", coltoa(cf));
send_to_interp(cline);
update(TRUE);
}
break;
// scroll
case 'z':
if ( bs != 2 ) break;
int scroll = 0;
switch (buf->pnext->value) {
case 'l':
scroll_right(1);
//unselect_ranges();
break;
case 'h':
scroll_left(1);
//unselect_ranges();
break;
case 'H':
scroll = calc_offscr_sc_cols();
if (atoi(get_conf_value("half_page_scroll"))) scroll /= 2;
scroll_left(scroll);
//unselect_ranges();
break;
case 'L':
scroll = calc_offscr_sc_cols();
if (atoi(get_conf_value("half_page_scroll"))) scroll /= 2;
scroll_right(scroll);
//unselect_ranges();
break;
case 'm':
;
int i = 0, c = 0, ancho = rescol;
offscr_sc_cols = 0;
for (i = 0; i < curcol; i++) {
for (c = i; c < curcol; c++) {
if (!col_hidden[c]) ancho += fwidth[c];
if (ancho >= (COLS - rescol)/ 2) {
ancho = rescol;
break;
}
}
if (c == curcol) break;
}
offscr_sc_cols = i;
break;
case 'z':
case '.':
case 't':
case 'b':
if (buf->pnext->value == 'z' || buf->pnext->value == '.')
scroll = currow - offscr_sc_rows + LINES - RESROW - 2 - (LINES - RESROW - 2)/2; // zz
else if (buf->pnext->value == 't')
scroll = currow - offscr_sc_rows + 1;
else if (buf->pnext->value == 'b')
scroll = currow - offscr_sc_rows - LINES + RESROW + 2;
if (scroll > 0)
scroll_down(scroll);
// else if (scroll > offscr_sc_rows)
// scroll_up(-scroll);
else if (scroll < 0)
scroll_up(-scroll);
// else if (offscr_sc_rows > 0)
// scroll_up(offscr_sc_rows);
break;
}
update(TRUE);
break;
// scroll up a line
case ctl('y'):
scroll_up(1);
update(TRUE);
break;
// scroll down a line
case ctl('e'):
scroll_down(1);
update(TRUE);
break;
// undo
case 'u':
#ifdef UNDO
do_undo();
// sync_refs();
EvalAll();
update(TRUE);
break;
#else
scerror("Build was done without UNDO support");
#endif
// redo
case ctl('r'):
#ifdef UNDO
do_redo();
// sync_refs();
EvalAll();
update(TRUE);
break;
#else
scerror("Build was done without UNDO support");
#endif
case '{': // left align
case '}': // right align
case '|': // center align
{
int p, r = currow, c = curcol, rf = currow, cf = curcol;
struct srange * sr;
if ( (p = is_range_selected()) != -1) {
sr = get_range_by_pos(p);
r = sr->tlrow;
c = sr->tlcol;
rf = sr->brrow;
cf = sr->brcol;
}
if (any_locked_cells(r, c, rf, cf)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
#ifdef UNDO
create_undo_action();
#endif
if (buf->value == '{') sprintf(interp_line, "leftjustify %s", v_name(r, c));
else if (buf->value == '}') sprintf(interp_line, "rightjustify %s", v_name(r, c));
else if (buf->value == '|') sprintf(interp_line, "center %s", v_name(r, c));
if (p != -1) sprintf(interp_line + strlen(interp_line), ":%s", v_name(rf, cf));
#ifdef UNDO
copy_to_undostruct(r, c, rf, cf, 'd');
#endif
send_to_interp(interp_line);
#ifdef UNDO
copy_to_undostruct(r, c, rf, cf, 'a');
end_undo_action();
#endif
cmd_multiplier = 0;
update(TRUE);
break;
}
case ctl('l'):
/*
endwin();
start_screen();
clearok(stdscr, TRUE);
update(TRUE);
flushinp();
show_header(input_win);
show_celldetails(input_win);
wrefresh(input_win);
update(TRUE);
*/
winchg();
break;
case '@':
EvalAll();
update(TRUE);
break;
// increase or decrease numeric value of cell or range
case '-':
case '+':
{
int r, c, tlrow = currow, tlcol = curcol, brrow = currow, brcol = curcol;
if ( is_range_selected() != -1 ) {
struct srange * sr = get_selected_range();
tlrow = sr->tlrow;
tlcol = sr->tlcol;
brrow = sr->brrow;
brcol = sr->brcol;
}
if (any_locked_cells(tlrow, tlcol, brrow, brcol)) {
scerror("Locked cells encountered. Nothing changed");
return;
}
if (atoi(get_conf_value("numeric")) == 1) goto numeric;
struct ent * p;
#ifdef UNDO
create_undo_action();
#endif
int arg = cmd_multiplier;
int mf = modflg; // keep original modflg
for (r = tlrow; r <= brrow; r++) {
for (c = tlcol; c <= brcol; c++) {
p = *ATBL(tbl, r, c);
if ( ! p ) {
continue;
} else if (p->expr && !(p->flags & is_strexpr)) {
//scerror("Can't increment / decrement a formula");
continue;
} else if (p->flags & is_valid) {
#ifdef UNDO
copy_to_undostruct(r, c, r, c, 'd');
#endif
p->v += buf->value == '+' ? (double) arg : - 1 * (double) arg;
#ifdef UNDO
copy_to_undostruct(r, c, r, c, 'a');
#endif
if (mf == modflg) modflg++; // increase just one time
}
}
}
#ifdef UNDO
end_undo_action();
#endif
if (atoi(get_conf_value("autocalc"))) EvalAll();
cmd_multiplier = 0;
update(TRUE);
}
break;
// input of numbers
default:
numeric:
if ( (isdigit(buf->value) || buf->value == '-' || buf->value == '+') &&
atoi(get_conf_value("numeric")) ) {
insert_edit_submode='=';
chg_mode(insert_edit_submode);
inputline_pos = 0;
ins_in_line(buf->value);
show_header(input_win);
}
}
return;
}
task main()
{
waitForStart(); // Wait for the tele-op period to begin
nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm
servoTarget(clawL) = 225; // Initialize the claw to be open
servoTarget(clawR) = 60;
PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun
wait1Msec(10); // Wait one tenth of a second
while (true)
{
getJoystickSettings(joystick); // Read the value of the joysticks
nxtDisplayTextLine(6, "Encoder: %d", nMotorEncoder[motorLift1]); // Display the value of the arm encoder
// The following code makes the directional pad on controller 1 give precise digital control of the drivetrain:
if (joystick.joy1_TopHat == 0) // If "up" on the directional pad is pressed:
{
go_forward(25); // Go forward at 25% speed
}
else if (joystick.joy1_TopHat == 1) // Else if "top right" on the directional pad is pressed:
{
strafe_forward_right(25); // Strafe diagonally forward and to the right at 25% speed
}
else if (joystick.joy1_TopHat == 2) // Else if "right" on the directional pad is pressed:
{
strafe_right(25); // Strafe to the right at 25% speed
}
else if (joystick.joy1_TopHat == 3) // Else if "bottom right" on the directional pad is pressed:
{
strafe_backward_right(25); // Strafe diagonally backward and to the right 25% speed
}
else if (joystick.joy1_TopHat == 4) // Else if "down" on the directional pad is pressed:
{
go_backward(25); // Go backward 25% speed
}
else if (joystick.joy1_TopHat == 5) // Else if "bottom left" on the directional pad is pressed:
{
strafe_backward_left(25); // Strafe diagonally backward and to the left 25% speed
}
else if (joystick.joy1_TopHat == 6) // Else if "left" on the directional pad is pressed:
{
strafe_left(25); // Strafe left 25% speed
}
else if (joystick.joy1_TopHat == 7) // Else if "top left" on the directional pad is pressed:
{
strafe_forward_left(25); // Strafe diagonally forward and to the left 25% speed
}
else if (joy1Btn(5)) // Else if button 6 is pressed:
{
rotate_clockwise(25); // Rotate the robot clockwise 25% speed
}
else if (joy1Btn(6)) // Else if button 5 is pressed:
{
rotate_counter_clockwise(25); // Rotate the robot counter-clockwise 25% speed
}
// The following code makes the joysticks on controller 1 give analogue control of the drivetrain:
else
{
motor[motorFL] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 + joystick.joy1_x2); // Scale the motors to the average
motor[motorFR] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 - joystick.joy1_x2); //of the left joystick Y value and
motor[motorBR] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 - joystick.joy1_x2); //the right joystick X value
motor[motorBL] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 + joystick.joy1_x2);
}
// The following code assigns the shoulder buttons on controller 2 to open and close the claw:
if (joy2Btn(5))
{
servoTarget(clawL) = 55; // If button 5 is pressed on controller 2, close the claw
servoTarget(clawR) = 170;
}
else if (joy2Btn(6))
{
servoTarget(clawL) = 115; // If button 6 is pressed on controller 2, open the claw
servoTarget(clawR) = 100;
}
else if (joy2Btn(8))
{
servoTarget(clawL) = ServoValue(clawL) + 1; // While button 7 is held, slowly close the claw
servoTarget(clawR) = ServoValue(clawR) - 1;
}
else if (joy2Btn(7))
{
servoTarget(clawL) = ServoValue(clawL) - 1; // While button 8 is held, slowly open the claw
servoTarget(clawR) = ServoValue(clawR) + 1;
}
else
{
servoTarget(clawL) = ServoValue(clawL); // Otherwise, do not move the claw
servoTarget(clawR) = ServoValue(clawR);
}
// The following code gives analogue control of the arm with the joystick, and assigns buttons 1-4 to move the arm to their respective rows on the scoring rack
if (joy2Btn(1)) // If button 1 is pressed, move the arm to the lowest row on the scoring rack
{
moveArm(level1Value);
}
else if (joy2Btn(2)) // If button 2 is pressed, move the arm to the middle row on the scoring rack
{
moveArm(level2Value);
}
else if (joy2Btn(3)) // If button 3 is pressed, move the arm to the top row on the scoring rack
{
moveArm(level3Value);
}
else if (joy2Btn(4)) // If button 4 is pressed, lower the arm all the way
{
moveArm(0);
}
else
{
motor[motorLift1] = (joystick.joy2_y1 / 12); // Otherwise, control the arm with joystick 1 on controller 2
motor[motorLift1] = (joystick.joy2_y1 / 24); // Or control the arm at half speed with joystick 2 on controller 2
// Note: these values are divided by 12 and 24 to bring the arm speed down to a reasonable level
}
}
}
static HRESULT WINAPI InternetExplorer_GoForward(IWebBrowser2 *iface)
{
InternetExplorer *This = impl_from_IWebBrowser2(iface);
TRACE("(%p)\n", This);
return go_forward(&This->doc_host);
}
void go_forward(void) {
int j, k, cand, visited = 0;
cell *p, *act_cell, *pre_cell, *neigh_cell;
int i, act_num, pre_num, neigh_num;
neightab *act_neigh, *next_neigh;
int delta;
act_cell = stack[stack_end].cl;
act_num = stack[stack_end].num;
act_neigh = &act_cell->neightab_array [act_num];
pre_cell = stack[stack_end - 1].cl;
pre_num = stack[stack_end - 1].num;
p = stack[0].cl;
i = stack[0].num;
/* Test whether neighbour atoms of actual atom
are vertices of possible ring */
for ( j=0; j<act_neigh->n; j++ ) {
/* cell and number of neighbour atom */
neigh_cell = (cell *) act_neigh->cl [j];
neigh_num = act_neigh->num[j];
/* Check whether neighbour has already been visited */
visited = 0;
for ( k=1; k<=stack_end; k++)
if ( (neigh_cell == stack[k].cl) && (neigh_num == stack[k].num) )
visited = 1;
/* Neighbour has not been visited. Disregard rings of length 2 */
if ( visited == 0 && (neigh_cell != p || neigh_num != i || stack_end > 1 ) ) {
/* Is path a closed ring ? */
if ( neigh_cell == p && neigh_num == i ) {
/* Test whether ring is shortest path ring */
if ( sp_ring() ) {
++histogram[stack_end+1];
++total_rings;
}
}
/* Path is not closed */
else {
/* Push new vertex on stack */
++stack_end;
stack[stack_end].cl = neigh_cell;
stack[stack_end].num = neigh_num;
/* Check path for unimodularity.
delta = 1: path length increases,
delta = 0: path length does not change,
delta = -1: path length decreases */
delta = neigh_cell->hops[neigh_num] - act_cell->hops[act_num];
if ( delta == 1 ) {
if ( stack[stack_end-1].status == 1 ) {
cand = 1;
stack[stack_end].status = 1;
}
else if ( stack[stack_end-1].status == 0 ) {
cand = 0;
stack[stack_end].status = 0;
}
else if ( stack[stack_end-1].status == -1 ) {
cand = 0;
stack[stack_end].status = -1;
}
}
else if (delta == 0 ) {
if ( stack[stack_end-1].status == 1 ) {
cand = 1;
stack[stack_end].status = 0;
}
else if ( stack[stack_end-1].status == 0 ) {
cand = 0;
stack[stack_end].status = 0;
}
else if (stack[stack_end-1].status == -1 ) {
cand = 0;
stack[stack_end].status = -1;
}
}
else if ( delta == -1 ) {
if ( stack[stack_end-1].status == 1 ) {
cand = 1;
stack[stack_end].status = -1;
}
else if (stack[stack_end-1].status == 0 ) {
cand = 1;
stack[stack_end].status = -1;
}
else if (stack[stack_end-1].status == -1 ) {
cand = 1;
stack[stack_end].status = -1;
}
}
/* Next vertex is a candidate for a path vertex */
if ( cand == 1 ) {
next_neigh = &neigh_cell->neightab_array[neigh_num];
/* If there are further neighbours and the distance is not greater
than max_length/2, iterate ring search */
if ( next_neigh->n > 1 && stack_end <= max_length && neigh_cell->hops[neigh_num] <= max_length/2 ) {
go_forward();
}
else {
/* Only one neighbour or path longer than max_length/2 */
stack_end--;
}
}
else {
/* Path not unimodular */
stack_end--;
}
} /* no ring found */
} /* neighbour is different from previous atoms. Not ring of length 2 */
}
stack_end--;
}
void search_rings(void) {
cell *p, *act_cell;
int ic, jc, kc, intv, stars = 0, count = 0;
int i, act_num, j;
neightab *pre_neigh, *act_neigh;
int icc, jcc, kcc, m;
cell *q;
/* Status line */
intv = natoms / 50;
printf("\nSearching rings.\n");
printf("|--------------------------------------------------|\n|");
fflush(stdout);
/* Initialize histogram */
if ( (histogram = (int *)malloc((max_length+1)*sizeof(int))) == NULL )
error("Cannot allocate memory for histogram!\n");
for( i=0; i<=max_length; i++)
histogram[i] = 0;
/* Allocate memory for stack */
stack = (atom * ) malloc( (max_length+1) * sizeof(atom) );
if ( stack == NULL )
error("Cannot allocate memory for stack!");
/* For each cell */
for (ic=0; ic < cell_dim.x; ++ic)
for (jc=0; jc < cell_dim.y; ++jc)
#ifndef TWOD
for (kc=0; kc < cell_dim.z; ++kc)
#endif
{
#ifdef TWOD
p = PTR_2D_V(cell_array,ic,jc ,cell_dim);
#else
p = PTR_3D_V(cell_array,ic,jc,kc,cell_dim);
#endif
/* For each atom in cell */
for ( i=0; i<p->n; ++i ) {
/* Initialize neighbour tables */
for (icc=0; icc < cell_dim.x; ++icc)
for (jcc=0; jcc < cell_dim.y; ++jcc)
#ifndef TWOD
for (kcc=0; kcc < cell_dim.z; ++kcc)
#endif
{
#ifdef TWOD
q = PTR_2D_V(cell_array,icc,jcc ,cell_dim);
#else
q = PTR_3D_V(cell_array,icc,jcc,kcc,cell_dim);
#endif
for ( m=0; m<q->n; ++m )
if ( q->del[m] == 0 )
q->color[m] = -1;
}
/* Compute distances of neighbours */
/* First atom has distance 0 and gets grey */
p->hops[i] = 0;
p->color[i] = 0;
/* Put base atom into queue (for computation of distances) */
queue = queue_create( p, i);
queue_length = 1;
/* Compute distances from atom p,i */
compute_hops();
/* Push base atom onto stack */
stack[0].cl = p;
stack[0].num = i;
stack[0].status = 1;
stack_end = 0;
/* Neighbour table of base atom */
pre_neigh = &p->neightab_array [i];
/* For all neighbour atoms */
for ( j=0; j<pre_neigh->n; j++ ) {
/* Cell and number of first neighbour atom */
act_cell = (cell *) pre_neigh->cl [j];
act_num = pre_neigh->num[j];
/* Push first neighbour atom onto stack */
stack[1].cl = act_cell;
stack[1].num = act_num;
stack[1].status = 1;
stack_end = 1;
/* status = 1: distance from base atom increases,
status = 0: distance from base atom does not change,
status = -1: distance from base atom decreases */
/* Neighbour table of first neighbour atom */
act_neigh = &act_cell->neightab_array [act_num];
/* If first neighbour has more than one neighbour, go on */
if ( act_neigh->n > 1 )
go_forward();
}
/* Remove atom p,i from system (i.e., mark it)
and update neighbour tables */
p->del[i] = 1;
update_neighbour_tables( p, i );
/* Status line */
count++;
if ( count > intv ) {
printf("*");
fflush(stdout);
count = 0;
stars++;
}
}
}
/* Status line */
if ( stars <= 50 )
for ( i=0; i<(50-stars); i++)
printf("*");
printf("|\n");
if ( total_rings > 0 )
printf("Total number of rings found: %d\n\n",total_rings/2);
else
printf("No rings found.\n\n");
}
Direction Runner::generate_step() {
Direction move;
//сымый простой кейс, мы всегда стараемся ползти вперед
if (hit == false && current_status.up != BlockType::WALL) {
move = go_forward();
}
//если уперлись в стену тупо повернем вправо
else if (hit == false && current_status.up == BlockType::WALL) {
hit = true;
rotate_right();
//если мы повернулись и стены там нет, все ок туда и идем
/* 1
0 0
*/
if (check_front() != BlockType::WALL) {
move = go_forward();
}
//в ином случае повернем еще направо и поползем
/*
1
0 1
*/
else {
rotate_right();
move = go_forward();
}
}
//тут описан обход стены в правом направлении
else if (hit == true) {
//тут чекнем возможность левого поворота
//он возможен, когда на прошлом ходу по левую руку была 1ка, а сейчас по левую руку 0
/*
1 0
0 0
мы во второй строке втором нуле, о предъидущем положении мы помним
*/
if (rotation_posibility() == true) {
rotate_left();
move = go_forward();
}
//если он невозможен
else {
//если спереди все чисто
/*
1 1
0 0
*/
if (check_front() != BlockType::WALL) {
move = go_forward();
}
//теперь обработаю случай с тупиком
/*
1 1
1 0
*/
if (check_front() == BlockType::WALL) {
//повернем направо
rotate_right();
//если там тупика нет то все ок
/*
1 1
1 0 0
*/
if (check_front() != BlockType::WALL) {
move = go_forward();
}
//в ином случае повернем еще направо и поползем назад
/*
1 1
1 0 1
*/
else {
rotate_right();
move = go_forward();
}
}
}
// надо проверить, обошли ли мы преграду
if (turn_counter == 0) {
hit = false;
}
}
last_face = face;
last_status = current_status;
return move;
}
/******************************************************************
* Main-function
*/
int main(void) {
/**************************************
* Set the clock to run at 80 MHz
*/
SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);
/**************************************
* Enable the floating-point-unit
*/
FPUEnable();
// We also want Lazystacking, so enable that too
FPULazyStackingEnable();
// We also want to put numbers close to zero to zero
FPUFlushToZeroModeSet(FPU_FLUSH_TO_ZERO_EN);
/**************************************
* Init variables
*/
uint16_t mapData[MAP_DATA_SIZE];
uint8_t stepDir = 0;
/**************************************
* Init peripherals used
*/
bluetooth_init();
init_stepper(); // Init the GPIOs used for the stepper and loading LED
InitI2C1(); // Init the communication with the lidar-unit through I2C
InitPWM();
setupTimers();
/**************************************
* State 2
*/
// Disable the timers that are used
disableTimer(TIMER1_BASE);
disableTimer(TIMER2_BASE);
// Enable all interrupts
IntMasterEnable();
/**************************************
* State 3
*/
// Indicate we should start with a scan regardless of what other things we have already got
// from UART-interrupt
// This means setting the appropriate bit in the status vector
stat_vec |= TAKE_MEAS;
/**************************************
* State 4
*/
// Contains main-loop where decisions should be made
for ( ; ; ) {
/**********************************
* Decision tree
*/
// Highest priority case first
// Check both interrupts at each iteration in the loop
if ( int_vec & UART_INT ) {
// Reset the indication
int_vec &= ~UART_INT;
// Remove drive-stop flag to enable movement
stat_vec &= ~DRIVE_STOP;
// Init data array
uint8_t dataArr[MAX_UART_MSG_SIZE];
// Collect the message
if ( readUARTMessage(dataArr, MAX_UART_MSG_SIZE) < SUCCESS ) {
// If we have recieved more data than fits in the vector we should simply
// go in here again and grab data
int_vec |= UART_INT;
}
// We have gathered a message
// and now need to determine what the message is
parseMsg(dataArr, MAX_UART_MSG_SIZE);
}
// Checking drive (movement) interrupt
if ( int_vec & TIMER2_INT ) {
int_vec &= ~TIMER2_INT;
// Disable TIMER2
disableTimer(TIMER2_BASE);
// Set drive-stop in status vector
stat_vec |= DRIVE_STOP;
}
// Checking measure interrupt
if ( int_vec & TIMER1_INT ) {
int_vec &= ~TIMER1_INT;
// Disable TIMER1
disableTimer(TIMER1_BASE);
// Take reading from LIDAR
mapData[stepCount++] = readLidar();
SysCtlDelay(2000);
// Take step
// Note: We need to take double meas at randvillkor (100) !!!!!
if ( stepCount > 0 && stepCount < 100 ) {
stepDir = 1;
}
else if ( stepCount >= 100 && stepCount < 200) {
stepDir = 0;
}
else {
stepDir = 1;
stepCount = 0;
// Reset busy-flag
stat_vec &= ~TAKE_MEAS;
}
step = takeStep(step, stepDir);
// Request reading from LIDAR
reqLidarMeas();
if ( stat_vec & TAKE_MEAS ) {
// Restart TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY));
}
else {
sendUARTDataVector(mapData, MAP_DATA_SIZE);
stat_vec &= ~BUSY;
}
}
// Check the drive_stop flag, which always should be set unless we should move
if ( stat_vec & DRIVE_STOP ) {
// Stop all movement
SetPWMLevel(0,0);
halt();
// MAKE SURE all drive-flags are not set
stat_vec &= ~(DRIVE_F | DRIVE_L | DRIVE_R | DRIVE_LL | BUSY);
}
// Should we drive?
else if ( stat_vec & DRIVE ) {
// Remove drive flag
stat_vec &= ~DRIVE;
// Increase PWM
increase_PWM(0,MAX_FORWARD_SPEED,0,MAX_FORWARD_SPEED);
if ( stat_vec & DRIVE_F ) {
enableTimer(TIMER2_BASE, DRIVE_FORWARD_TIME);
}
else if ( stat_vec & DRIVE_LL ) {
enableTimer(TIMER2_BASE, DRIVE_TURN_180_TIME);
}
else {
enableTimer(TIMER2_BASE, DRIVE_TURN_TIME);
}
}
if ( !(stat_vec & BUSY) ) {
// Tasks
switch ( stat_vec ) {
case ((uint8_t)DRIVE_F) :
// Call drive function
go_forward();
// Set the drive flag & BUSY
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_L) :
// Call drive-left function
go_left();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_R) :
// Call drive-right function
go_right();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_LL) :
// Call turn 180-degrees function
go_back();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)TAKE_MEAS) :
// Request reading from LIDAR
reqLidarMeas();
// Start TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY)); // if sysclock = 1 s, 1/120 = 8.3 ms
// We are busy
stat_vec |= BUSY;
break;
default:
break;
}
}
}
}
void move()
{
int i;
/*variable to use in figuring out the "best" option*/
int max_q_score = 0;
/*what do we do next? store it here*/
/*we init to -1 as an error*/
int next_movement = -1;
/*Where we started.*/
/*We don't use ROTATION_2 all the way through in case it changes.*/
int initial_angle = norm_rotation(ROTATION_2);
/*Where we ended up.*/
int new_angle;
/*Show the current angle*/
cputc_native_user(CHAR_A, CHAR_N, CHAR_G, CHAR_L); // ANGL
msleep(200);
lcd_int(initial_angle);
msleep(500);
/*
* Most of the time, we do the "correct" thing
* by finding the best q_score of our possible options.
* On the off chance that norm_random() is low (or EPSILON is high ;)
* we then "explore" by choosing a random movement.
*/
if(norm_random() > EPSILON_CURRENT)
{
/*We are doing what the table tells us to.*/
cputc_native_user(CHAR_r, CHAR_e, CHAR_a, CHAR_l); // real
msleep(500);
for(i=0; i<MOVEMENTS; i++)
{
if(q_score[initial_angle][i] > max_q_score)
{
max_q_score = q_score[initial_angle][i];
next_movement = i;
}
}
}
else
{
double temp;
/*We are just picking something at random.*/
cputc_native_user(CHAR_r, CHAR_a, CHAR_n, CHAR_d); // rand
msleep(500);
/*pick one. Any one.*/
temp = norm_random();
next_movement = temp*MOVEMENTS;
/*show what we do next*/
lcd_int(next_movement);
sleep(1);
}
/*what happens if next_movement never gets changed?*/
/*we'd hate to do HARD_LEFT over and over again*/
/*so we choose randomly*/
if(-1==next_movement)
{
double temp;
temp = norm_random();
next_movement = temp*MOVEMENTS;
}
/*having chosen a movement, lets do it*/
switch(next_movement)
{
case HARD_LEFT:
cputc_native_user(CHAR_H, CHAR_L, 0, 0); // HL
hard_left();
break;
case SOFT_LEFT:
cputc_native_user(CHAR_S, CHAR_L, 0, 0); // SL
soft_left();
break;
case FORWARD:
cputc_native_user(CHAR_F, CHAR_W, CHAR_W, CHAR_D); // FWD
go_forward();
break;
case SOFT_RIGHT:
cputc_native_user(CHAR_S, CHAR_R, 0, 0); // SR
soft_right();
break;
case HARD_RIGHT:
cputc_native_user(CHAR_H, CHAR_R, 0, 0); // HR
hard_right();
break;
case REVERSE:
cputc_native_user(CHAR_R, CHAR_E, CHAR_V, 0); // REV
go_back();
break;
default:
/*this is an error and should never be reached*/
cputc_native_user(CHAR_E, CHAR_R, CHAR_R, 0); // ERR
stop_motors();
sleep(1);
break;
}
/*Once we've started, we'd better stop*/
stop_motors();
/*Allows us to read direction*/
msleep(500);
/*This is here just to make the next function cleaner*/
new_angle = norm_rotation(ROTATION_2);
/*Where are we now?*/
cputc_native_user(CHAR_N, CHAR_E, CHAR_W, CHAR_W); // NEW
msleep(200);
lcd_int(new_angle);
msleep(500);
/*
* Since we know that "next_movement" took us from "initial_angle"
* to new_angle (ROTATION_2), we store that increased probability.
*/
steering_results[initial_angle][next_movement][new_angle] += ALPHA;
/*here we re-norm so that the sum of the probabilities is still 1*/
for(i=0; i<ANGLES; i++)
{
steering_results[initial_angle][next_movement][i] /= (1+ALPHA);
}
/*The last thing we do is reduce Epsilon*/
if(EPSILON_CURRENT > EPSILON_MIN)
{
EPSILON_CURRENT-=EPSILON_DECAY;
}
}
// go backward (@see go_forward)
void go_backward(Angle_t angle, Angle_t *phi, Angle_t *theta)
{
go_forward(- angle, phi, theta);
}