int askforOutput(void){
int continueFlag = 0, choice = 0;
clearLCD();
// print the prompt
goToLine(1);
LCDprintln("***************");
goToLine(2);
LCDprintln("What output?");
goToLine(3);
LCDprintln("-> Xbee [Left]");
goToLine(4);
LCDprintln("-> USB [Right]");
goToLine(5);
LCDprintln("-> N/A [Center]");
goToLine(6);
LCDprintln("***************");
//resetting the buttons to up position
reset_buttons();
do{
// if left is pushed
if(LEFT_BUTTON_PRESSED){
// XBEE is the choice
choice = 1;
continueFlag = 1;
// if the right is pushed
}else if(RIGHT_BUTTON_PRESSED){
// USB Connection is the choice
choice = 2;
continueFlag = 1;
// if the middle is pushed
}else if(MIDDLE_BUTTON_PRESSED){
// No output is the choice
choice = 3;
continueFlag = 1;
}
//do while an input is noticed
}while(continueFlag == 0);
// reset the buttons again
reset_buttons();
// return the choice
return choice;
}
static uint8_t model_save_xml (char *name, float x, float y, int8_t button, float data, uint8_t action) {
reset_buttons();
FILE *fr;
char tmp_str[128];
sprintf(tmp_str, "mkdir -p %s/models", get_datadirectory());
system(tmp_str);
sprintf(tmp_str, "%s/models/%s", get_datadirectory(), name);
fr = fopen(tmp_str, "wb");
if (fr != 0) {
fprintf(fr, "<rcflow>\n");
fprintf(fr, " <name>%s</name>\n", ModelData[ModelActive].name);
fprintf(fr, " <image>%s</image>\n", ModelData[ModelActive].image);
fprintf(fr, " <type>%s</type>\n", dronetypes[ModelData[ModelActive].dronetype]);
fprintf(fr, " <telemetry>\n");
fprintf(fr, " <type>%s</type>\n", teletypes[ModelData[ModelActive].teletype]);
fprintf(fr, " <device>%s</device>\n", ModelData[ModelActive].telemetry_port);
fprintf(fr, " <baud>%i</baud>\n", ModelData[ModelActive].telemetry_baud);
if (strstr(ModelData[ModelActive].telemetry_port, "rfcomm") > 0) {
fprintf(fr, " <bluetooth_addr>%s</bluetooth_addr>\n", ModelData[ModelActive].telebtaddr);
fprintf(fr, " <bluetooth_pin>%s</bluetooth_pin>\n", ModelData[ModelActive].telebtpin);
}
fprintf(fr, " </telemetry>\n");
mavlink_xml_save(ModelActive, fr);
mwi21_xml_save(ModelActive, fr);
openpilot_xml_save(fr);
fprintf(fr, "</rcflow>\n");
fclose(fr);
}
return 0;
}
int askforUSB(void){
int continueFlag = 0, choice = 0;
clearLCD();
// print the prompt
goToLine(1);
LCDprint("***************");
goToLine(2);
LCDprint("Record?");
goToLine(3);
LCDprint("-> YES [Left]");
goToLine(4);
LCDprint("-> NO [Right]");
goToLine(5);
LCDprint("***************");
//resetting the buttons to up position
reset_buttons();
do{
// if left is pushed
if(LEFT_BUTTON_PRESSED){
// Yes record
choice = 1;
continueFlag = 1;
// if the right is pushed
}else if(RIGHT_BUTTON_PRESSED){
// no do not record
choice = 0;
continueFlag = 1;
// if the middle is pushed
}
//do while an input is noticed
}while(continueFlag == 0);
// reset the buttons again
reset_buttons();
// return the choice
return choice;
}
static uint8_t model_image_change (char *name, float x, float y, int8_t button, float data, uint8_t action) {
reset_buttons();
char tmp_str[128];
sprintf(tmp_str, "%s", BASE_DIR);
filesystem_set_dir(tmp_str);
filesystem_set_callback(model_image_set);
filesystem_reset_filter();
filesystem_add_filter(".png\0");
filesystem_set_mode(setup.view_mode);
return 0;
}
static uint8_t model_load (char *name, float x, float y, int8_t button, float data, uint8_t action) {
reset_buttons();
char tmp_str[128];
sprintf(tmp_str, "%s/models", get_datadirectory());
filesystem_set_dir(tmp_str);
filesystem_set_callback(model_load_xml);
filesystem_reset_filter();
filesystem_add_filter(".xml\0");
filesystem_set_mode(setup.view_mode);
return 0;
}
uint8_t UAVT_gui_scroll(char *name, float x, float y, int8_t button, float data, uint8_t action) {
if (button == 4) {
if (sy > 0) {
sy--;
}
} else if (button == 5) {
sy++;
}
reset_buttons();
return 0;
}
uint8_t model_save (char *name, float x, float y, int8_t button, float data, uint8_t action) {
char tmp_str[200];
reset_buttons();
keyboard_set_callback(model_save_xml);
if (strstr(ModelData[ModelActive].name, ".xml\0") > 0) {
strncpy(tmp_str, ModelData[ModelActive].name, 199);
} else {
sprintf(tmp_str, "%s.xml", ModelData[ModelActive].name);
}
keyboard_set_text(tmp_str);
keyboard_set_mode(setup.view_mode);
return 0;
}
void screen_overview(ESContext *esContext) {
#ifndef SDLGL
ESMatrix modelview;
UserData *userData = esContext->userData;
#endif
char tmp_str[1024];
uint8_t n = 0;
#ifndef SDLGL
esMatrixLoadIdentity(&modelview);
esMatrixMultiply(&userData->mvpMatrix, &modelview, &userData->perspective);
#endif
reset_buttons();
draw_text_button(esContext, "Screens", setup.view_mode, "Screens", FONT_PINK, -0.8, -0.6 + -2 * 0.1, 0.002, 0.08, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)0);
for (n = 0; n < VIEW_MODE_LAST; n++) {
sprintf(tmp_str, "%s", view_names[n]);
if (setup.view_mode == n) {
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_GREEN, -0.8, -0.6 + n * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
} else {
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_WHITE, -0.8, -0.6 + n * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
}
}
draw_text_button(esContext, "Options", setup.view_mode, "Options", FONT_PINK, 0.8, -0.6 + -2 * 0.1, 0.002, 0.08, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)0);
n = 0;
if (setup.speak == 1) {
draw_text_button(esContext, "SPEAK", setup.view_mode, "SPEAK", FONT_GREEN, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "SPEAK", setup.view_mode, "SPEAK", FONT_WHITE, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
if (logmode == 1) {
draw_text_button(esContext, "LOGGING", setup.view_mode, "LOGGING", FONT_GREEN, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "LOGGING", setup.view_mode, "LOGGING", FONT_WHITE, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
#ifndef OSX
if (logplay == 1) {
draw_text_button(esContext, "LOGPLAYER", setup.view_mode, "LOGPLAYER", FONT_GREEN, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "LOGPLAYER", setup.view_mode, "LOGPLAYER", FONT_WHITE, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
#endif
n++;
n++;
n++;
draw_text_button(esContext, "EXIT", setup.view_mode, "EXIT", FONT_WHITE, 0.8, -0.6 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
static uint8_t model_image_set (char *name, float x, float y, int8_t button, float data, uint8_t action) {
strncpy(ModelData[ModelActive].image, name, 511);
reset_buttons();
return 0;
}
static uint8_t model_load_xml (char *name, float x, float y, int8_t button, float data, uint8_t action) {
model_parseDoc(name);
reset_buttons();
return 0;
}
void screen_overview(ESContext *esContext) {
#ifndef SDLGL
ESMatrix modelview;
UserData *userData = esContext->userData;
#endif
char tmp_str[1024];
uint8_t n = 0;
#ifndef SDLGL
esMatrixLoadIdentity(&modelview);
esMatrixMultiply(&userData->mvpMatrix, &modelview, &userData->perspective);
#endif
reset_buttons();
int x = 0;
int y = 0;
for (n = 0; n < VIEW_MODE_LAST; n++) {
if (x >= 4) {
x = 0;
y += 1;
}
#ifndef WINDOWS
#ifndef ANDROID
draw_to_buffer();
if (n == VIEW_MODE_HUD) {
screen_hud(esContext);
} else if (n == VIEW_MODE_TELEMETRY) {
screen_telemetry(esContext);
} else if (n == VIEW_MODE_MODEL) {
screen_model(esContext);
} else if (n == VIEW_MODE_RCFLOW) {
screen_rcflow(esContext);
} else if (n == VIEW_MODE_FMS) {
screen_fms(esContext);
} else if (n == VIEW_MODE_WPEDIT) {
screen_wpedit(esContext);
} else if (n == VIEW_MODE_MAP) {
screen_map(esContext, lat, lon, zoom);
} else if (n == VIEW_MODE_SYSTEM) {
screen_system(esContext);
#ifndef ANDROID
} else if (n == VIEW_MODE_VIDEOLIST) {
screen_videolist(esContext);
#ifdef USE_TCL
} else if (n == VIEW_MODE_TCL) {
screen_tcl(esContext);
#endif
#endif
} else if (n == VIEW_MODE_TRACKER) {
screen_tracker(esContext);
} else if (n == VIEW_MODE_FCMENU) {
if (ModelData[ModelActive].teletype == TELETYPE_MULTIWII_21) {
screen_mwi_menu(esContext);
} else if (ModelData[ModelActive].teletype == TELETYPE_BASEFLIGHT) {
screen_mwi_menu(esContext);
} else if (ModelData[ModelActive].teletype == TELETYPE_GPS_NMEA) {
screen_graph(esContext);
} else if (ModelData[ModelActive].teletype == TELETYPE_OPENPILOT) {
screen_openpilot(esContext);
} else {
screen_mavlink_menu(esContext);
}
}
draw_to_screen();
draw_buffer_to_screen(-1.422 + x * 0.71, -0.99 + y * 0.66, -1.422 + x * 0.71 + 0.71, -0.99 + y * 0.66 + 0.66, 0.002, 1.0);
sprintf(tmp_str, "%s", view_names[n]);
if (setup.view_mode == n) {
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_GREEN, -1.422 + 0.35 + x * 0.71, -0.99 + y * 0.66, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
} else {
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_WHITE, -1.422 + 0.35 + x * 0.71, -0.99 + y * 0.66, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
}
#else
sprintf(tmp_str, "%s", view_names[n]);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_WHITE, -1.422 + 0.35 + x * 0.71, -0.99 + y * 0.66 + 0.3, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
#endif
#else
sprintf(tmp_str, "%s", view_names[n]);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str, FONT_WHITE, -1.422 + 0.35 + x * 0.71, -0.99 + y * 0.66 + 0.3, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)n);
#endif
reset_buttons();
x++;
}
x = 0;
y = 0;
for (n = 0; n < VIEW_MODE_LAST; n++) {
if (x >= 4) {
x = 0;
y += 1;
}
// draw_rect_f3(esContext, -1.3 + x * 0.5, -0.8 + y * 0.4, 0.002, -1.3 + x * 0.5 + 0.5, -0.8 + y * 0.4 + 0.4, 0.002, 255, 255, 255, 255);
// set_button(view_names[n], setup.view_mode, -1.3 + x * 0.5, -0.8 + y * 0.4, -1.3 + x * 0.5 + 0.5, -0.8 + y * 0.4 + 0.4, overview_set, (float)n, 0);
draw_rect_f3(esContext, -1.422 + x * 0.71, -0.99 + y * 0.66, 0.002, -1.422 + x * 0.71 + 0.71, -0.99 + y * 0.66 + 0.66, 0.002, 255, 255, 255, 255);
set_button(view_names[n], setup.view_mode, -1.422 + x * 0.71, -0.99 + y * 0.66, -1.422 + x * 0.71 + 0.71, -0.99 + y * 0.66 + 0.66, overview_set, (float)n, 0);
x++;
}
draw_text_button(esContext, "Options", setup.view_mode, "Options", FONT_PINK, 1.05, 0.55 + -2 * 0.1, 0.002, 0.08, ALIGN_CENTER, ALIGN_TOP, overview_set, (float)0);
n = 0;
if (setup.speak == 1) {
draw_text_button(esContext, "SPEAK", setup.view_mode, "SPEAK", FONT_GREEN, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "SPEAK", setup.view_mode, "SPEAK", FONT_WHITE, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
if (logmode == 1) {
draw_text_button(esContext, "LOGGING", setup.view_mode, "LOGGING", FONT_GREEN, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "LOGGING", setup.view_mode, "LOGGING", FONT_WHITE, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
#ifndef OSX
if (logplay == 1) {
draw_text_button(esContext, "LOGPLAYER", setup.view_mode, "LOGPLAYER", FONT_GREEN, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "LOGPLAYER", setup.view_mode, "LOGPLAYER", FONT_WHITE, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
#endif
if (clientmode != 1 && setup.webport != 0) {
if (webserv_is_running() == 1) {
draw_text_button(esContext, "WEBSERV", setup.view_mode, "stop Webserver", FONT_GREEN, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
} else {
draw_text_button(esContext, "WEBSERV", setup.view_mode, "start Webserver", FONT_WHITE, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
}
draw_text_button(esContext, "EXIT", setup.view_mode, "EXIT", FONT_GREEN, 1.05, 0.45 + n++ * 0.1, 0.002, 0.06, ALIGN_CENTER, ALIGN_TOP, option_cmd, 0.0);
}
uint8_t mwi_view_change (char *name, float x, float y, int8_t button, float data) {
mwi_view = 1 - mwi_view;
reset_buttons();
return 0;
}
void screen_keyboard(ESContext *esContext) {
#ifdef SDLGL
if (draw_target() != 0) {
return;
}
#endif
#ifndef SDLGL
ESMatrix modelview;
UserData *userData = esContext->userData;
#endif
if (show_keyboard != setup.view_mode) {
return;
}
reset_buttons();
draw_box_f3(esContext, -1.5, -1.0, 0.02, 1.5, 1.0, 0.02, 0, 0, 0, 200);
draw_title(esContext, "Keyboard");
char tmp_str[100];
char tmp_str2[100];
int n = 0;
#ifndef SDLGL
esMatrixLoadIdentity(&modelview);
esMatrixMultiply(&userData->mvpMatrix, &modelview, &userData->perspective);
esMatrixMultiply(&userData->mvpMatrix2, &modelview, &userData->perspective);
#endif
for (n = 0; n < strlen(new_name) + 1; n++) {
if (new_name[n] != 0) {
sprintf(tmp_str2, "%c", new_name[n]);
} else {
sprintf(tmp_str2, "[END]");
}
sprintf(tmp_str, "set_char_%i", n);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -0.9 + n * 0.08, -0.6, 0.02, 0.06, ALIGN_LEFT, ALIGN_TOP, keyboard_pos_char, n);
if (n == new_name_cnt) {
draw_text_button(esContext, "mark", setup.view_mode, "^", FONT_WHITE, -0.9 + n * 0.08, -0.5, 0.02, 0.06, ALIGN_LEFT, ALIGN_TOP, keyboard_pos_char, n);
}
}
draw_text_button(esContext, "rcname_save", setup.view_mode, "[OK]", FONT_WHITE, 0.4, -0.6, 0.02, 0.06, ALIGN_LEFT, ALIGN_TOP, keyboard_name_save, 0);
draw_text_button(esContext, "rcname_cancel", setup.view_mode, "[CANCEL]", FONT_WHITE, 0.6, -0.6, 0.02, 0.06, ALIGN_LEFT, ALIGN_TOP, keyboard_name_cancel, 0);
uint8_t x = 0;
uint8_t y = 0;
if (type == 0) {
for (n = 33; n < 150; n++) {
sprintf(tmp_str2, "%c", n);
sprintf(tmp_str, "add_char_%i", n);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -1.1 + x * 0.14, -0.3 + y * 0.14, 0.02, 0.06, ALIGN_CENTER, ALIGN_TOP, keyboard_add_char, n);
if (x > 14) {
y++;
x = 0;
} else {
x++;
}
}
} else {
for (n = '0'; n <= '9'; n++) {
sprintf(tmp_str2, "%c", n);
sprintf(tmp_str, "add_char_%i", n);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -1.1 + x * 0.14, -0.3 + y * 0.14, 0.02, 0.1, ALIGN_CENTER, ALIGN_TOP, keyboard_add_char, n);
if (x > 1) {
y++;
x = 0;
} else {
x++;
}
}
if (type == 2) {
sprintf(tmp_str2, "%c", '.');
sprintf(tmp_str, "add_char_%i", '.');
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -1.1 + x * 0.14, -0.3 + y * 0.14, 0.02, 0.1, ALIGN_CENTER, ALIGN_TOP, keyboard_add_char, '.');
if (x > 1) {
y++;
x = 0;
} else {
x++;
}
}
sprintf(tmp_str2, "%c", '-');
sprintf(tmp_str, "add_char_%i", '-');
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -1.1 + x * 0.14, -0.3 + y * 0.14, 0.02, 0.1, ALIGN_CENTER, ALIGN_TOP, keyboard_add_char, '-');
if (x > 1) {
y++;
x = 0;
} else {
x++;
}
}
sprintf(tmp_str2, "[END]");
sprintf(tmp_str, "add_char_%i", 0);
draw_text_button(esContext, tmp_str, setup.view_mode, tmp_str2, FONT_WHITE, -1.1 + x * 0.14, -0.3 + y * 0.14, 0.02, 0.06, ALIGN_CENTER, ALIGN_TOP, keyboard_add_char, 0);
if (keyboard_key[0] != 0) {
if (strcmp(keyboard_key, "return") == 0) {
keyboard_name_save("", 0.0, 0.0, 0, 0.0, 0);
} else if (strcmp(keyboard_key, "backspace") == 0) {
new_name_cnt--;
} else if (strcmp(keyboard_key, "delete") == 0) {
keyboard_add_char("", 0.0, 0.0, 0, 0.0, 0);
} else if (strcmp(keyboard_key, "up") == 0) {
} else if (strcmp(keyboard_key, "down") == 0) {
} else if (strcmp(keyboard_key, "end") == 0) {
} else if (strcmp(keyboard_key, "home") == 0) {
new_name_cnt = 0;
} else if (strcmp(keyboard_key, "escape") == 0) {
keyboard_name_cancel("", 0.0, 0.0, 0, 0.0, 0);
} else if (strcmp(keyboard_key, "left") == 0) {
new_name_cnt--;
} else if (strcmp(keyboard_key, "right") == 0) {
new_name_cnt++;
} else if (strcmp(keyboard_key, "space") == 0) {
keyboard_add_char("", 0.0, 0.0, 0, ' ', 0);
} else if (keyboard_key[1] == 0) {
keyboard_add_char("", 0.0, 0.0, 0, keyboard_key[0], 0);
}
keyboard_key[0] = 0;
}
}
int main()
{
set_a_button_text("pipe jumping");
set_b_button_text("center drive");
set_c_button_text("testing");
int strategy;
WAIT(a_button()||b_button()||c_button());
if(a_button())
{
strategy=PIPE_JUMP;
}
else if(b_button())
{
strategy=CENTER_DRIVE;
}
else//c
{
WAIT(side_button());
msleep(1000);
set_servo_position(TRIBBLE_ARM, TA_DOWN);
set_servo_position(TRIBBLE_CLAW, TC_OPEN);
set_servo_position(BLOCK_CLAW, BC_START);
set_servo_position(BASKET_ARM, BA_DOWN);
enable_servos();
msleep(2000);
forward(20, 60);
return 0;
//set_up_drive();//gets the servos and stuff in driving position
//test_driving();
reset_buttons();
set_up_drive();
printf("press black button to begin\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
back_line_follow_time(60, 5000);
return 9001;
}
WAIT(!a_button()&&!b_button());
set_a_button_text("light start");//only gets here if running one of the real code options
set_b_button_text("no ls");
set_c_button_text("-");
boolean l_s=false;//whether or not is doing light start
WAIT(a_button()||b_button());
if(a_button())//if b, does nothing-->l_s already false
l_s=true;
reset_buttons();
if(strategy==PIPE_JUMP)
set_up_jump();
else//drive
set_up_drive();
if(l_s)
{
light_start(LIGHT_SENSOR);
}
else//no light start
{
printf("press black button when ready\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
}
shutdownin(239.5);
start();//timing
if(strategy==PIPE_JUMP)
{
ready_to_jump();
time_drive(-90, -90, 1300);//jump!
servo_set(TRIBBLE_ARM, TA_UP,.3);//move the claw up so it can square up
time_drive(-50, -50, 1500);//get towards the pipe
physical_squareup(false);//square up on the back
time_drive(50, 50, 2500);//go back towards the other wall
physical_squareup(true);//square up on the front
time_drive(-50, -50, 1000);//get back to the center
right(87, 0, 50);//turn towards the edge
time_drive(-60, -60, 1500);//move towards edge
physical_squareup(false);//square up on the outside of the field
tribble_claw_drop();//drop the claw-->plow position
time_drive(68, 60, 1100);//arc into the wall to wall follow to the blocks
//forward(5.25, 60);//move to block position
grab_blocks();//grab the blocks...
forward(20, 60);//plow the tribbles!
servo_set(TRIBBLE_CLAW, TC_CLOSE, .7);//close claw to catch tribbles
servo_set(TRIBBLE_ARM, TA_UP, 1);//put the arm up to get across the center
forward(20, 60);//get across the center
servo_set(TRIBBLE_ARM, TA_START, .5);//push into the ground so the claw doesn't jump
servo_set(TRIBBLE_CLAW, TC_OPEN, .8);//back into plow position
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//back to drive position
msleep(250);
forward(7, 60);//get to dumping location
nowstr("first dump started at");
drive_dump();//dump...
forward(7, 60);//replow the tribbles (maybe get a few extra)
back(2, 60);//get to optimal grab location
tribble_claw_dump();//put the tribbles in the basket
back(5, 60);//back to dump location
tribble_claw_drop();//put the claw back down
drive_dump();//dump again
nowstr("first dump finished at");
//forward(15, 60);//plow the second set of tribbles, get to block location
time_drive(66, 60, 3000);//arc towards the right wall to make sure it follows the right wall
grab_blocks();//grab the blocks...
forward(8, 60);//plow the remaining tribbles
servo_set(TRIBBLE_CLAW, TC_CLOSE, .4);//grab the tribbles
servo_set(TRIBBLE_ARM, TA_DUMP, 1);//raise the arm (to allow it to square up)
time_drive(60, 60, 1500);//get towards the wall
physical_squareup(true);//and square up on it
msleep(200);
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//put the arm down to get it out of the way
nowstr("second dump started at");
drive_dump();//dump
tribble_claw_dump();//put the tribbles in the basket
tribble_claw_drop();//
drive_dump();//dump again
nowstr("second dump finished at");
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .3);//get the claw up and out of the way
servo_set(TRIBBLE_ARM, TA_UP, .3);
back(6, 60);//back up to get the caught tribbles
servo_set(TRIBBLE_ARM, TA_START, .3);//put the claw back down
servo_set(TRIBBLE_CLAW, TC_OPEN, .5);//
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//
forward(38, 60);//push the tribbles (get any that went way down the field)
back(2, 60);//tribbles near edge of claw
tribble_claw_dump();
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw, just for good measure
back_line_follow(30, 60);//get back to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the arm out of the way
nowstr("third dump started at");
drive_dump();
nowstr("third dump finished at");
back_line_follow(17, 60);//get back across the center
right(180, 0, 50);//turn around
tribble_claw_drop();//put the claw down...
forward(25, 60);//and plow!
back(2, 60);//optimal grab location
tribble_claw_dump();//well duh...
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw for good measure
time_drive(60, 60, 1500);//move towards the wall
physical_squareup(true);//and square up on it
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the claw out of the way
nowstr("fourth dump started at");
drive_dump();//dump stuff
move_block_arm(BLA_MID);//get the block arm back out of the way
servo_set(BASKET_ARM, BA_UP, 1);//put the basket back up
time_drive(-50, -50, 1500);//one last chance to dump (stay there just cause...you never know)
}
else//drive strategy
{
forward(38, 90);//get towards the center
right(90, 0, 70);//turn towards the middle of the gap
forward(17, 90);//get through the gap and to the black tape
left(85, 0, 70);//turn parallel to the black tape
servo_set(TRIBBLE_ARM, TA_JUMP, .4);//get the claw into plow position
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .4);//
servo_set(TRIBBLE_ARM, TA_DOWN, .4);//
servo_set(TRIBBLE_CLAW, TC_OPEN, .4);//
}
nowstr("finished at");
return 42;
}
uint8_t UAVT_gui_show_num_change (char *name, float x, float y, int8_t button, float data, uint8_t action) {
show_num = (uint16_t)data;
reset_buttons();
sy = 0;
return 0;
}
void swarm_draw_setup(ESContext *esContext) {
int n = 0;
int ny = 1;
int gx = 0;
int gy = 0;
char tmp_str[128];
char tmp_str2[128];
float px1 = -0.8;
float py1 = -0.9;
float px2 = 0.9;
float py2 = 0.1;
float sy = 0.08;
reset_buttons();
EntryList list[3];
list[0].name = "ROLE";
list[1].name = "OFFSET";
list[2].name = NULL;
draw_window(esContext, px1, py1, px2, py2, 0.002, "SwarmSetup.options", "Swarm-Setup", list, SwarmSetup.options, swarm_set);
if (SwarmSetup.options == 0) {
draw_text_button(esContext, "master_title", setup.view_mode, "Master:", FONT_WHITE, px1, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, 0.0);
ny++;
uint8_t n2 = 0;
EntryList list[MODELS_MAX + 2];
list[0].name = "GCS";
for (n2 = 0; n2 < MODELS_MAX; n2++) {
list[n2 + 1].name = ModelData[n2].name;
}
list[n2 + 1].name = NULL;
draw_pulldown(esContext, px1 + 0.1, py1 + (float)ny * sy, 0.45, 0.01, "SwarmSetup.master", list, pd_openm, SwarmSetup.master + 1, swarm_set);
ny++;
draw_text_button(esContext, "sclave_title", setup.view_mode, "Slaves:", FONT_WHITE, px1, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, 0.0);
ny++;
for (n = 0; n < 4; n++) {
sprintf(tmp_str2, "SwarmSetup.slave%i", n);
draw_pulldown(esContext, px1 + 0.1, py1 + (float)ny * sy, 0.44, 0.01, tmp_str2, list, pd_open[n], SwarmSetup.slave[n] + 1, swarm_set);
sprintf(tmp_str, "X=%im", SwarmSetup.offset_x[n]);
sprintf(tmp_str2, "SwarmSetup.offx%i", n);
draw_spinbox(esContext, px1 + 0.55, py1 + (float)ny * sy, 0.34, 0.01, tmp_str2, "X=%0.0fm", SwarmSetup.offset_x[n], swarm_set);
sprintf(tmp_str, "Y=%im", SwarmSetup.offset_y[n]);
sprintf(tmp_str2, "SwarmSetup.offy%i", n);
draw_spinbox(esContext, px1 + 0.9, py1 + (float)ny * sy, 0.34, 0.01, tmp_str2, "Y=%0.0fm", SwarmSetup.offset_y[n], swarm_set);
sprintf(tmp_str, "Z=%im", SwarmSetup.offset_z[n]);
sprintf(tmp_str2, "SwarmSetup.offz%i", n);
draw_spinbox(esContext, px1 + 1.25, py1 + (float)ny * sy, 0.34, 0.01, tmp_str2, "Z=%0.0fm", SwarmSetup.offset_z[n], swarm_set);
ny++;
}
ny++;
draw_text_button(esContext, "swarm_yawmode_", setup.view_mode, "Yaw-Rotate", FONT_GREEN, px1 + 0.1, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, 0.0);
draw_checkbox(esContext, px1 + 0.7, py1 + (float)ny * sy, 0.005, "swarm_yawmode", SwarmSetup.yaw_mode, swarm_set);
ny++;
draw_text_button(esContext, "swarm_rotate_", setup.view_mode, "Pattern-Rotate", FONT_GREEN, px1 + 0.1, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, 0.0);
draw_checkbox(esContext, px1 + 0.7, py1 + (float)ny * sy, 0.005, "swarm_rotate", SwarmSetup.rotate, swarm_set);
ny++;
} else {
float gx1 = px1 + 0.5;
float gx2 = px2 - 0.1;
float gy1 = py1 + 0.15;
float gy2 = py2 - 0.15;
int gxn = 80;
int gyn = 40;
ny++;
ny++;
sprintf(tmp_str, "SLAVE=%i", SwarmSetup.selected);
draw_text_button(esContext, "SwarmSetup.selected", setup.view_mode, tmp_str, FONT_GREEN, px1 + 0.01, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, 0.0);
ny++;
sprintf(tmp_str, "%s", ModelData[SwarmSetup.slave[SwarmSetup.selected]].name);
draw_text_button(esContext, "SwarmSetup.offn", setup.view_mode, tmp_str, FONT_GREEN, px1 + 0.01, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, (float)SwarmSetup.selected);
ny++;
sprintf(tmp_str, "X=%im", SwarmSetup.offset_x[SwarmSetup.selected]);
draw_text_button(esContext, "SwarmSetup.offx", setup.view_mode, tmp_str, FONT_GREEN, px1 + 0.01, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, (float)SwarmSetup.selected);
ny++;
sprintf(tmp_str, "Y=%im", SwarmSetup.offset_y[SwarmSetup.selected]);
draw_text_button(esContext, "SwarmSetup.offy", setup.view_mode, tmp_str, FONT_GREEN, px1 + 0.01, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, (float)SwarmSetup.selected);
ny++;
sprintf(tmp_str, "Z=%im", SwarmSetup.offset_z[SwarmSetup.selected]);
draw_text_button(esContext, "SwarmSetup.offz", setup.view_mode, tmp_str, FONT_GREEN, px1 + 0.01, py1 + (float)ny * sy, 0.005, 0.06, ALIGN_LEFT, ALIGN_TOP, swarm_set, (float)SwarmSetup.selected);
ny++;
for (gx = 0; gx <= gxn; gx++) {
draw_line_f3(esContext, gx1 + (float)gx * ((gx2 - gx1) / (float)gxn), gy1, 0.006, gx1 + (float)gx * ((gx2 - gx1) / (float)gxn), gy2, 0.006, 255, 255, 255, 90);
}
for (gy = 0; gy <= gyn; gy++) {
draw_line_f3(esContext, gx1, gy1 + (float)gy * ((gy2 - gy1) / (float)gyn), 0.006, gx2, gy1 + (float)gy * ((gy2 - gy1) / (float)gyn), 0.006, 255, 255, 255, 90);
}
draw_circleFilled_f3(esContext, gx1 + (gx2 - gx1) / 2.0, gy1 + (gy2 - gy1) / 2.0, 0.007, 0.01, 255, 0, 0, 255);
for (n = 0; n < 4; n++) {
if (SwarmSetup.slave[n] == -1) {
continue;
}
float offx = SwarmSetup.offset_x[n] / 50.0;
float offy = SwarmSetup.offset_y[n] / 50.0;
draw_circleFilled_f3(esContext, gx1 + (gx2 - gx1) / 2.0 + offx, gy1 + (gy2 - gy1) / 2.0 + offy, 0.007, 0.01, 255, 255, 0, 255);
if (n == SwarmSetup.selected) {
draw_circle_f3(esContext, gx1 + (gx2 - gx1) / 2.0 + offx, gy1 + (gy2 - gy1) / 2.0 + offy, 0.007, 0.02, 255, 0, 255, 255);
}
}
}
draw_buttonbox(esContext, "swarm_setup_done", setup.view_mode, "CLOSE", px2 - 0.33, py2 - 0.1, 0.3, 0.005, swarm_set, 0.0);
}