/*
* 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;
}
