/* * Store a value in a PP structure. * Input: * pp_pp* pp: pointer to the PP structure to store in * mpq_t value: the value to store * int parent: the parent to refer to * int self: TRUE if value represents 1/parent, FALSE if it represents the * resolution of pp[parent] * Returns: * Nothing. * Effect: * The specified value is stored in the specified PP structure, with a * reference to the specified parent. Internal variables are updated * accordingly. * * For a given value q, the caller is expected to have determined the * greatest prime power p^k dividing the denominator of q. In this case, * the pp structure should be that for p^k. * * The parent is used solely for determining a final solution vector. * If the value is a source value (i.e. 1/r representing the integer * r in the solution vector), the parent should be set to r, and self should * be TRUE. If not, the parent is assumed to be a fully resolved PP structure * storing a walk result that specifies the actual vector contribution * (possibly recursively), and self should be FALSE. */ void pp_save_any(pp_pp* pp, mpq_t value, int parent, int self) { int i; mpz_t zvalue; pp_value* v; ZINIT(&zvalue, "pp_save_any zvalue"); /* new value = mpq_numref(value) * (denominator / mpq_denref(value)) * new_pp() setting of denominator guarantees exactness */ mpz_divexact(zvalue, pp->denominator, mpq_denref(value)); mpz_mul(zvalue, zvalue, mpq_numref(value)); /* adding to total may change required fixed size, at worst once per pp */ mpz_add(pp->total, pp->total, zvalue); if (pp->total->_mp_size > pp->valnumsize) { Dprintf("pp_%d: growing to %d with total %Zd (denom %Zd)\n", pp->pp, pp->total->_mp_size, pp->total, pp->denominator); pp_grownum(pp, pp->total->_mp_size); } ++pp->valsize; pp_grow(pp, pp->valsize); v = pp_value_i(pp, pp->valsize - 1); v->parent = parent; v->self = self; v->inv = (pp->invdenom * mod_ui(zvalue, pp->p)) % pp->p; /* ref: MAX_P */ mpx_set_z(ppv_mpx(v), pp->valnumsize, zvalue); pp->invtotal = (pp->invtotal + v->inv) % pp->p; pp_insert_last(pp); ZCLEAR(&zvalue, "pp_save_any zvalue"); }
void new_pp(pp_pp* pp, int prime, int power) { int i, set; mpz_t reduced_denom; mpx_support* xsup; if (prime > MAX_P) { fprintf(stderr, "prime %d exceeds max %d\n", prime, MAX_P); exit(1); } pp->p = prime; pp->pp = power; if (pp->p == pp->pp) inverse_table(pp->p); pp_pushlist(pp); ZINIT(&pp->total, "pp_%d.total", pp->pp); ZINIT(&pp->denominator, "pp_%d.denominator", pp->pp); ZINIT(&pp->min_discard, "pp_%d.min_discard", pp->pp); QINIT(&pp->spare, "pp_%d.spare", pp->pp); /* required denominator is the lcm of (power, S) where S contains those * pp such that pp * power <= k. */ ZINIT(&reduced_denom, "new_pp reduced_denom"); set = 0; /* start from pplist[1], because we've already been inserted at [0] */ for (i = 1; i < pplistsize; ++i) { if (pplist[i]->pp * power <= k0) { mpz_mul_ui(pp->denominator, pplist[i]->denominator, power / mpz_gcd_ui(NULL, pplist[i]->denominator, power)); set = 1; break; } } if (!set) { /* power == 1 */ if (power != 1) { fprintf(stderr, "new_pp: did not find denominator for %d\n", power); exit(1); } mpz_set_ui(pp->denominator, 1); } mpz_divexact_ui(reduced_denom, pp->denominator, power); pp->invdenom = invfast(mod_ui(reduced_denom, pp->p), pp->p); ZCLEAR(&reduced_denom, "new_pp reduced_denom"); xsup = mpx_support_z(pp->denominator); pp->valnumsize = xsup->size; pp->adder = xsup->adder; pp->cmper = xsup->cmper; pp_grow(pp, MINPPSET); }
void initialize(){ float skew = 0;//find_skew(field_state.curr_loc); build_bisecting_points(skew, &bisecting_points); build_lever_pivot_points(&lever_pivot_points); build_territory_pivot_points(&territory_pivot_points); field_state.stage = FIRST_STAGE; field_state.drive_direction = BACKWARD; field_state.substage = PIVOT_SUBSTAGE; accelerate_time = DRIVE_ACCELERATE_TIME; field_state.pid_enabled = TRUE; decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST; field_state.stored_time = get_time(); field_state.curr_time = get_time(); target_vel = TARGET_CIRCLE_VEL; field_state.start_time = get_time(); uint8_t changed_last_update = FALSE; field_state.encoder_value = encoder_read(FREEWHEEL_ENCODER_PORT); field_state.balls_held = 0; current_vel = 0; update_field(); log_init(30000); uint8_t sextant = get_current_sextant(field_state.curr_loc); if(sextant == 0){ field_state.color = BLUE; } else if(sextant == 3){ field_state.color = RED; } else{ field_state.color = 2; } Location target_loc = get_territory_pivot_point_loc(mod_ui(sextant - 1, 6)); set_new_destination(field_state.curr_loc, target_loc); update_field(); servo_set_pos(0, 300); }
int main(int argc,char** args) { srand(time(NULL)); try { if (SDL_Init(SDL_INIT_VIDEO)) { fprintf(stderr,"Unable to initialize SDL: %s\n",SDL_GetError()); return EXIT_FAILURE; } atexit(SDL_Quit); SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER,1); #if 0 screen = SDL_SetVideoMode(1440,900,32,SDL_OPENGL|SDL_FULLSCREEN); #else screen = SDL_SetVideoMode(1024,768,32,SDL_OPENGL/*|SDL_FULLSCREEN*/); #endif if(!screen) { fprintf(stderr,"Unable to create SDL screen: %s\n",SDL_GetError()); return EXIT_FAILURE; } SDL_WM_SetCaption("GlestNG","GlestNG"); GLenum err = glewInit(); if(GLEW_OK != err) { fprintf(stderr, "Error: %s\n", glewGetErrorString(err)); return EXIT_FAILURE; } fprintf(stdout, "Status: Using GLEW %s\n", glewGetString(GLEW_VERSION)); // we have a GL context so we can go ahead and init all the singletons std::auto_ptr<fs_t> fs_settings(fs_t::create("data/")); fs_t::settings = fs_settings.get(); // set it globally std::auto_ptr<xml_parser_t> xml_settings(new xml_parser_t("UI Settings",fs_settings->get_body("ui_settings.xml"))); xml_settings->set_as_settings(); std::auto_ptr<graphics_t::mgr_t> graphics_mgr(graphics_t::create()); std::auto_ptr<fonts_t> fonts(fonts_t::create()); std::auto_ptr<fs_t> fs; try { fs.reset(fs_t::create("data/Glest")); if(false) { fs_file_t::ptr_t logo_file(fs_settings->get("logo.g3d")); istream_t::ptr_t logostream(logo_file->reader()); logo = std::auto_ptr<model_g3d_t>(new model_g3d_t(*logostream)); } load(*fs); } catch(glest_exception_t* e) { std::cerr << "cannot load glest data: " << e << std::endl; delete e; } std::auto_ptr<ui_mgr_t> ui_(ui_mgr()); std::auto_ptr<mod_ui_t> mod_ui(mod_ui_t::create()); std::auto_ptr<terrain_t> terrain(terrain_t::gen_planet(5,500,3)); //world()->dump(std::cout); v4_t light_amb(0,0,0,1), light_dif(1.,1.,1.,1.), light_spec(1.,1.,1.,1.), light_pos(1.,1.,-1.,0.), mat_amb(.7,.7,.7,1.), mat_dif(.8,.8,.8,1.), mat_spec(1.,1.,1.,1.); glLightfv(GL_LIGHT0,GL_AMBIENT,light_amb.v); glLightfv(GL_LIGHT0,GL_DIFFUSE,light_dif.v); glLightfv(GL_LIGHT0,GL_SPECULAR,light_spec.v); glLightfv(GL_LIGHT0,GL_POSITION,light_pos.v); glLightfv(GL_LIGHT1,GL_AMBIENT,light_amb.v); glLightfv(GL_LIGHT1,GL_DIFFUSE,light_dif.v); glLightfv(GL_LIGHT1,GL_SPECULAR,light_spec.v); glMaterialfv(GL_FRONT,GL_AMBIENT,mat_amb.v); glMaterialfv(GL_FRONT,GL_DIFFUSE,mat_dif.v); glMaterialfv(GL_FRONT,GL_SPECULAR,mat_spec.v); glMaterialf(GL_FRONT,GL_SHININESS,100.0); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_LIGHT1); glDepthFunc(GL_LEQUAL); glEnable(GL_DEPTH_TEST); glAlphaFunc(GL_GREATER,0.4); glEnable(GL_COLOR_MATERIAL); glEnable(GL_RESCALE_NORMAL); glEnable(GL_BLEND); glEnable(GL_TEXTURE_2D); glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE); glEnable(GL_COLOR_MATERIAL); glEnable(GL_NORMALIZE); glFrontFace(GL_CW); camera(); bool quit = false; SDL_Event event; SDL_EnableKeyRepeat(200,20); SDL_EnableUNICODE(true); const unsigned start = SDL_GetTicks(); framerate.reset(); while(!quit) { set_now(SDL_GetTicks()-start); while(!quit && SDL_PollEvent(&event)) { if(ui_mgr()->offer(event)) continue; switch (event.type) { case SDL_MOUSEMOTION: if(selection) std::cout << "drag" << std::endl; /*printf("Mouse moved by %d,%d to (%d,%d)\n", event.motion.xrel, event.motion.yrel, event.motion.x, event.motion.y);*/ break; case SDL_MOUSEBUTTONDOWN: click(event.button.x,event.button.y); if(selection) std::cout << "selection: "<<selected_point<<std::endl; break; case SDL_MOUSEBUTTONUP: if(selection) std::cout << "selection stopped" << std::endl; selection = false; break; case SDL_KEYDOWN: switch(event.key.keysym.sym) { case SDLK_PLUS: zoom += 1; camera(); break; case SDLK_MINUS: zoom -= 1; camera(); break; case SDLK_ESCAPE: quit = true; break; case SDLK_m: // MODDING MODE if(!fs.get()) { std::cerr << "(modding menu triggered but mod not loaded)" << std::endl; break; } if(mod_ui->is_shown()) mod_ui->hide(); else mod_ui->show(ref_t(*techtree,TECHTREE,techtree->name)); break; default: std::cout << "Ignoring key " << (int)event.key.keysym.scancode << "," << event.key.keysym.sym << "," << event.key.keysym.mod << "," << event.key.keysym.unicode << std::endl; } break; case SDL_QUIT: quit = true; break; } } framerate.tick(now()); tick(); } for(tests_t::iterator i=objs.begin(); i!=objs.end(); i++) delete *i; return EXIT_SUCCESS; } catch(data_error_t* de) { std::cerr << "Oh! " << de << std::endl; } catch(graphics_error_t* ge) { std::cerr << "Oh! " << ge << std::endl; } catch(panic_t* panic) { std::cerr << "Oh! " << panic << std::endl; } return EXIT_FAILURE; }
void update_field(){ copy_objects(); int current_x = (int)(game.coords[0].x * VPS_RATIO); int current_y = (int)(game.coords[0].y * VPS_RATIO); uint8_t has_encoder_moved = FALSE; /*if(get_time() > field_state.last_encoder_update + ENCODER_UPDATE_TIME){ if(encoder_read(FREEWHEEL_ENCODER_PORT) != field_state.encoder_value){ field_state.robot_stopped = TRUE; field_state.encoder_value = encoder_read(FREEWHEEL_ENCODER_PORT); field_state. } else{ field_state.robot_stopped = TRUE; } }*/ //printf("Current X: %d, Current Y: %d\n", current_x, current_y); float current_theta = (((float)game.coords[0].theta) * 180.0) / 2048; field_state.curr_loc_plus_delta.x = current_x; //+ get_delta(field_state.curr_loc).x; field_state.curr_loc_plus_delta.y = current_y; //+ get_delta(field_state.curr_loc).y; uint8_t sextant = get_current_sextant(field_state.curr_loc_plus_delta); uint8_t old_sextant = get_current_sextant(field_state.curr_loc); //printf("Current Distance: %f, Waypoint Distance: %f, Target Distance: %f, Distance Increment: %f\n", pythagorean(field_state.curr_loc_plus_delta.x, field_state.curr_loc_plus_delta.y), pythagorean(field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y), pythagorean(field_state.target_loc.x, field_state.target_loc.y), field_state.distance_increment); float dist_to_waypoint = pythagorean_loc(field_state.curr_loc_plus_delta, field_state.target_loc_waypoint); if(sextant != old_sextant ){//|| (get_angle_error_circle() > 90 && dist_to_waypoint < CIRCLE_DECELERATE_DISTANCE_SECOND){ uint8_t target_sextant = get_current_sextant(field_state.target_loc); /*int direction; if((get_angle_error_circle() > 90 && dist_to_waypoint < CIRCLE_DECELERATE_DISTANCE_SECOND) && (sextant == old_sextant)){ //we're about to spiral...let's not spiral direction = get_waypoint_direction(field_state.curr_loc_plus_delta, field_state.target_loc, gyro_get_degrees()); if(get_sextant_difference(field_state.curr_loc_plus_delta, field_state.target_loc, direction) > 1){ //we aren't at the target sextant yet, so set target waypoint to next waypoint float distance_increment; switch(direction){ case COUNTER_CLOCKWISE: if(target_sextant < sextant) target_sextant += 6; new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint); field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant+4)%12], bisecting_points[(2*sextant+5)%12], new_distance); break; case CLOCKWISE: if(target_sextant > sextant) target_sextant -= 6; new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint); field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[mod_ui(2*sextant - 2, 12)], bisecting_points[mod_ui(2*sextant - 1, 12)], new_distance); break; } } } }*/ //We have broken the plane, get next waypoint, or target if in same sextant int direction = get_waypoint_direction(field_state.curr_loc_plus_delta, field_state.target_loc, gyro_get_degrees()); if(sextant != target_sextant){ float distance_increment; if(sextant == mod_ui(old_sextant + direction, 6)){ float new_distance; switch(direction){ case COUNTER_CLOCKWISE: if(target_sextant < sextant) target_sextant += 6; new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint); field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant+2)%12], bisecting_points[(2*sextant+3)%12], new_distance); break; case CLOCKWISE: if(target_sextant > sextant) target_sextant -= 6; new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint); field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant)%12], bisecting_points[(2*sextant + 1)%12], new_distance); break; } } field_state.target_loc_plane = get_scaled_loc(field_state.target_loc_waypoint, OUTER_HEX_APATHEM_DIST); } else{ field_state.target_loc_waypoint = field_state.target_loc; } field_state.curr_loc = field_state.curr_loc_plus_delta; encoder_reset(LEFT_ENCODER_PORT); encoder_reset(RIGHT_ENCODER_PORT); } if((field_state.substage == TERRITORY_APPROACH_SUBSTAGE) && current_vel > 0){ if(get_time() - field_state.stored_time > TERRITORY_TIMEOUT_TIME || game.coords[0].score != field_state.score){ if(!(field_state.we_want_to_dump)){ field_state.substage = DRIVE_SUBSTAGE; //SET MOTORS TO LEVER ARC THINGY target_vel = 96; current_vel = 96; set_spinners(0); //accelerate_time = CIRCLE_ACCELERATE_TIME; //decelerate_distance = CIRCLE_DECELERATE_DISTANCE; //KP_CIRCLE = 1.5; //set_motors(55, 120); field_state.stored_time = get_time(); field_state.drive_direction = FORWARD; field_state.pid_enabled = TRUE; field_state.circle_PID.enabled = true; servo_set_pos(1, SERVO_UP); set_new_destination(field_state.curr_loc_plus_delta, get_lever_pivot_point_loc(sextant)); } else{ retreat_from_territory(); } } if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){ field_state.we_want_to_dump = TRUE; } } if((field_state.substage == LEVER_APPROACH_SUBSTAGE) && current_vel > 0){ if(get_time() - field_state.stored_time > LEVER_APPROACH_TIME){ field_state.substage = LEVER_SUBSTAGE; //SET MOTORS TO LEVER ARC THINGY //target_vel = 64; //current_vel = 64; //set_spinners(0); //accelerate_time = CIRCLE_ACCELERATE_TIME; //decelerate_distance = CIRCLE_DECELERATE_DISTANCE; //KP_CIRCLE = 1.5; //set_motors(55, 120); stop_motors(); field_state.stored_time = get_time(); field_state.target_loc_waypoint = field_state.curr_loc_plus_delta; field_state.target_loc = field_state.curr_loc_plus_delta; field_state.pid_enabled = TRUE; field_state.stored_time = get_time(); int score_temp = field_state.score; servo_set_pos(1, SERVO_DOWN); pause(CLICKY_CLICKY_TIME + 100); while(1){ copy_objects(); if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){ field_state.we_want_to_dump = TRUE; } if(game.coords[0].score != score_temp){ score_temp += 40; field_state.balls_held += 1; } if(game.coords[0].score == field_state.score + 200 || field_state.stored_time + LEVER_TIMEOUT_TIME < get_time() ){ if(!field_state.we_want_to_dump){ pause(50); field_state.substage = DRIVE_SUBSTAGE; target_vel = TARGET_CIRCLE_VEL; current_vel = 0; accelerate_time = CIRCLE_ACCELERATE_TIME; decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST; KP_CIRCLE = KP_DRIVE; field_state.stored_time = get_time(); field_state.start_drive_time = get_time(); field_state.drive_direction = BACKWARD; field_state.circle_PID.enabled = true; field_state.pid_enabled = true; uint8_t temp_sextant = sextant; if(temp_sextant == 0) temp_sextant += 6; set_new_destination(field_state.curr_loc_plus_delta, get_territory_pivot_point_loc(get_target_territory(field_state.curr_loc_plus_delta))); break; } else{ pause(50); get_your_ass_to_a_toilet(); break; } } servo_set_pos(1, SERVO_MIDDLE); pause(CLICKY_CLICKY_TIME); servo_set_pos(1, SERVO_DOWN); pause(CLICKY_CLICKY_TIME); } } } /*if(field_state.substage == TRANSITION_SUBSTAGE){ if(get_time() - field_state.stored_time > TRANSITION_TIME){ field_state.substage = LEVER_SUBSTAGE; stop_motors(); field_state.stored_time = get_time(); field_state.target_loc_waypoint = field_state.curr_loc_plus_delta; field_state.target_loc = field_state.curr_loc_plus_delta; field_state.pid_enabled = TRUE; field_state.circle_PID.enabled = true; while(1){ copy_objects(); if(game.coords[0].score == field_state.score + 200 || field_state.stored_time + LEVER_TIMEOUT_TIME < get_time() ){ field_state.substage = DRIVE_SUBSTAGE; target_vel = 128; current_vel = 0; accelerate_time = CIRCLE_ACCELERATE_TIME; decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST; KP_CIRCLE = KP_DRIVE; field_state.stored_time = get_time(); field_state.start_drive_time = get_time(); field_state.drive_direction = BACKWARD; field_state.circle_PID.enabled = true; uint8_t temp_sextant = sextant; if(temp_sextant == 0) temp_sextant += 6; set_new_destination(field_state.curr_loc_plus_delta, get_territory_pivot_point_loc((temp_sextant - 1)%6)); break; } servo_set_pos(1, SERVO_DOWN); pause(CLICKY_CLICKY_TIME); servo_set_pos(1, SERVO_MIDDLE); pause(CLICKY_CLICKY_TIME); } } }*/ /*else if((field_state.substage == LEVER_APPROACH_SUBSTAGE) && current_vel > 0){ if(get_time() - field_state.stored_time > TERRITORY_TIMEOUT_TIME || game.coords[0].score != field_state.score){ field_state.substage = LEVER_APPROACH_SUBSTAGE; target_vel = 64; current_vel = 0; set_spinners(0); //accelerate_time = CIRCLE_ACCELERATE_TIME; //decelerate_distance = CIRCLE_DECELERATE_DISTANCE; //KP_CIRCLE = 1.5; //field_state.start_drive_time = get_time(); field_state.drive_direction = FORWARD; set_new_destination(field_state.curr_loc_plus_delta, get_lever_robot_loc((sextant)%6)); } }*/ if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){ if(field_state.substage == DRIVE_SUBSTAGE){ if(!field_state.we_want_to_dump) get_your_ass_to_a_toilet(); } field_state.we_want_to_dump = TRUE; } int acceptable_error = get_acceptable_error(field_state.substage); float dist_to_target = pythagorean(field_state.target_loc.x - field_state.curr_loc_plus_delta.x, field_state.target_loc.y - field_state.curr_loc_plus_delta.y); if(field_state.target_loc.x == field_state.target_loc_waypoint.x && field_state.target_loc.y == field_state.target_loc_waypoint.y){ if(dist_to_target < acceptable_error){ gyro_set_degrees(current_theta); encoder_reset(LEFT_ENCODER_PORT); encoder_reset(RIGHT_ENCODER_PORT); /*if(field_state.substage == TERRITORY_APPROACH_SUBSTAGE){ field_state.substage = TERRITORY_SUBSTAGE; stop_motors(); set_spinners(0); current_vel = 0; target_vel = 0; } if(field_state.substage == LEVER_APPROACH_SUBSTAGE){ field_state.substage = LEVER_SUBSTAGE; stop_motors(); current_vel = 0; target_vel = 0; }*/ if(field_state.substage == DRIVE_SUBSTAGE || field_state.substage == TERRITORY_RETREAT_SUBSTAGE || field_state.substage == LEVER_RETREAT_SUBSTAGE){ if(field_state.stage == FIRST_STAGE) field_state.stage = SECOND_STAGE; if(field_state.target_loc.x == get_territory_pivot_point_loc(sextant).x && field_state.target_loc.y == get_territory_pivot_point_loc(sextant).y){ if(field_state.substage != TERRITORY_RETREAT_SUBSTAGE){ set_new_destination(field_state.curr_loc_plus_delta, get_territory_robot_loc(sextant)); set_spinners( 229 * field_state.color); target_vel = 64; current_vel = 0; field_state.start_drive_time = get_time(); accelerate_time = WALL_ACCELERATE_TIME; decelerate_distance = (int)(WALL_DECELERATE_DISTANCE); field_state.substage = TERRITORY_APPROACH_SUBSTAGE; KP_CIRCLE = KP_APPROACH; field_state.drive_direction = BACKWARD; } else{ field_state.substage = PIVOT_SUBSTAGE; uint8_t dump_sextant = pick_dump_sextant(); Location dump_loc = get_dump_location_robot(dump_sextant); set_new_destination(field_state.curr_loc_plus_delta, dump_loc); current_vel = 0; } } else if(field_state.target_loc.x == get_lever_pivot_point_loc(sextant).x && field_state.target_loc.y == get_lever_pivot_point_loc(sextant).y){ if(field_state.substage != LEVER_RETREAT_SUBSTAGE){ target_vel = 64; current_vel = 0; set_new_destination(field_state.curr_loc_plus_delta, get_lever_robot_loc(sextant)); decelerate_distance = (int)(WALL_DECELERATE_DISTANCE); field_state.start_drive_time = get_time(); accelerate_time = WALL_ACCELERATE_TIME; KP_CIRCLE = KP_APPROACH; field_state.drive_direction = FORWARD; field_state.substage = LEVER_APPROACH_SUBSTAGE; } else{ field_state.substage = PIVOT_SUBSTAGE; //WE WILL PROBABLY NEVER USE LEVER_RETREAT_SUBSTAGE... } } else if(field_state.target_loc.x == get_dump_location_robot(sextant).x && field_state.target_loc.y == get_dump_location_robot(sextant).y){ field_state.substage = DUMPING_SUBSTAGE; set_new_destination(field_state.curr_loc_plus_delta, get_dump_location(sextant)); current_vel = 0; target_vel = 64; field_state.start_dump_time = get_time(); decelerate_distance = 0; KP_CIRCLE = KP_APPROACH; field_state.drive_direction = BACKWARD; } } } } run_pivot_subroutine(); if(field_state.substage == DUMPING_SUBSTAGE){ if(get_time() - field_state.start_dump_time > 4000) servo_set_pos(0, 150); if(get_time() - field_state.start_dump_time > 4000 + time_during_dump){ servo_set_pos(0, 300); } } if(current_x != field_state.curr_loc.x || current_y != field_state.curr_loc.y || current_theta != field_state.curr_angle){ field_state.update_time = get_time(); field_state.curr_loc.x = current_x; field_state.curr_loc.y = current_y; field_state.curr_angle = current_theta; //printf("Lag: %lu, CX: %d, CY: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_time() - field_state.stored_time, current_x, current_y, field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT)); //field_state.stored_time = get_time(); encoder_reset(LEFT_ENCODER_PORT); encoder_reset(RIGHT_ENCODER_PORT); if(field_state.substage == DRIVE_SUBSTAGE) gyro_set_degrees(current_theta); } field_state.curr_time = get_time(); //***TIMEOUTS*** /*if(field_state.curr_time - field_state.update_time >= DRIVE_TIMEOUT_TIME && !is_decelerating() && field_state.substage == DRIVE_SUBSTAGE && dist_to_target < acceptable_error){ //We hit a wall...or another robot, but we haven't coded robot-ramming timeouts yet... float distance = pythagorean(field_state.curr_loc_plus_delta.x, field_state.curr_loc_plus_delta.y); if(fabs(distance - get_min_distance_loc_param(field_state.curr_loc)) < fabs(distance - get_max_distance_loc_param(field_state.curr_loc))){ //We hit the inner hex } }*/ field_state.score = game.coords[0].score; }