int umain(){
robot_id = 1;
copy_objects();
copy_objects();
field_state.curr_loc.x = (int)(game.coords[0].x * VPS_RATIO);
field_state.curr_loc.y = (int)(game.coords[0].y * VPS_RATIO);
field_state.curr_angle = (((float)game.coords[0].theta) * 180.0) / 2048;
gyro_set_degrees(field_state.curr_angle);
initialize();
/*for(int sextant = 0; sextant < 6; sextant++){
printf("TPX: %d, TPY: %d, LPX: %d, LPY: %d\n", get_territory_pivot_point_loc(sextant).x, get_territory_pivot_point_loc(sextant).y, lever_pivot_points[2*sextant], lever_pivot_points[2*sextant + 1]);
}*/
init_pid(&field_state.circle_PID, KP_CIRCLE, KI_CIRCLE, KD_CIRCLE, &get_angle_error_circle, &update_circle_velocities);
field_state.circle_PID.enabled = true;
field_state.circle_PID.goal = 0;
//printf("OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", 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));
while(1){
update_field();
accelerate();
decelerate();
if(field_state.pid_enabled)
update_pid(&field_state.circle_PID);
if(get_time() > 119000){
}
//printf("Sextant: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_current_sextant(field_state.curr_loc), 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));
}
return 0;
}
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 load_configuration_file (FILE* config_file,CONFIG* config_struct ){
char line[257] ;
int cur_val ;
char cur_param [256] ;
int x = 0 ;
while(fgets(line,256,config_file) !=NULL){
if (strlen(line) <= 1 || contain_ch (line,'=')==false )
continue ;
cur_val = get_param_value(line,cur_param);
x= update_field(config_struct,cur_param,cur_val) ;
assert(x!=0);
}
return 1 ;
}
void
MBlockChunk::recv(MBlockDataMsg *dm)
{
if(dm->seqnum == update.wait_seqnum) {
DBG( "recv: handling ("<<dm->seqnum<<")" )
update_field(dm); // update the appropriate field value
if(update.nRecd==nRecvPatches) {
update.wait_seqnum = -1; //This update is complete
thread->resume();
}
} else if (dm->seqnum>seqnum) {
DBG( "recv: stashing early ("<<dm->seqnum<<")" )
CmmPut(messages, 1, &(dm->seqnum), dm);
} else
CkAbort("MBLOCK MBlockChunk received message from the past!\n");
}
int form(char *banner, app_input_t form_questions[], size_t num_fields)
{
char *update_tok;
page_option pg_opt;
if (g_page_idx > g_last_page_completed + 1) {
printf("You must complete the previous page before proceeding to this page\n");
g_page_idx = g_last_page_completed + 1;
return -1;
}
int i;
int unfinished = FALSE;
for (i=0; i < num_fields; i++) {
if (form_questions[i].input == NULL) {
unfinished = TRUE;
g_last_page_completed = g_page_idx - 1;
break;
}
}
if ( g_page_idx == g_last_page_completed + 1 || unfinished) {
pg_opt = fill_out_form(form_questions, num_fields);
if (pg_opt == NOOP) {
g_last_page_completed++;
} else if (pg_opt == UPDATE) {
printf("Cannot update field until all fields are inputted\n");
return pg_opt;
} else {
return pg_opt;
}
}
print_page(banner, form_questions, num_fields);
printf("\nType **next to continue\n");
pg_opt = get_response();
if (pg_opt == UPDATE) {
update_tok = g_user_resp;
strsep(&update_tok, " ");
if (update_field(update_tok, form_questions, num_fields) != 0)
printf("Update Unsuccessful\n");
}
return pg_opt;
}
void main()
{
init_device ();
while (1) {
if (out_of_game) {
if (!input (BTN_A) || !input (BTN_B)) {
dir_left = field = 1;
out_of_game = 0;
output_low (LED_OUT);
}
}
else
update_field ();
delay_ms (300);
}
}
crm_time_t *
parse_xml_duration(crm_time_t * start, xmlNode * duration_spec)
{
crm_time_t *end = NULL;
const char *value = NULL;
end = crm_time_new(NULL);
crm_time_set(end, start);
update_field("years", crm_time_add_years);
update_field("months", crm_time_add_months);
update_field("weeks", crm_time_add_weeks);
update_field("days", crm_time_add_days);
update_field("hours", crm_time_add_hours);
update_field("minutes", crm_time_add_minutes);
update_field("seconds", crm_time_add_seconds);
return end;
}
ha_time_t *
parse_xml_duration(ha_time_t * start, xmlNode * duration_spec)
{
ha_time_t *end = NULL;
const char *value = NULL;
end = new_ha_date(FALSE);
ha_set_time(end, start, TRUE);
update_field("years", add_years);
update_field("months", add_months);
update_field("weeks", add_weeks);
update_field("days", add_days);
update_field("hours", add_hours);
update_field("minutes", add_minutes);
update_field("seconds", add_seconds);
return end;
}
/**
* Builds UPDATE statement where cmd->valss specify column name-value pairs
* and cmd->match specify WHERE clause.
* @param sql_cmd SQL statement as a result of this function
* @param cmd input for statement creation
*/
static int build_update_cmd(str* sql_cmd, db_cmd_t* cmd)
{
struct string_buffer sql_buf = {.s = NULL, .len = 0, .size = 0, .increment = 128};
db_fld_t* fld;
int i;
int rv = 0;
str tmpstr;
rv = sb_add(&sql_buf, &strings[STR_UPDATE]); /* "UPDATE " */
rv |= sb_add(&sql_buf, &cmd->table); /* table name */
rv |= sb_add(&sql_buf, &strings[STR_SET]); /* " SET " */
/* column name-value pairs */
for(i = 0, fld = cmd->vals; !DB_FLD_LAST(fld[i]); i++) {
rv |= sb_add(&sql_buf, set_str(&tmpstr, fld[i].name));
rv |= sb_add(&sql_buf, set_str(&tmpstr, " = "));
rv |= sb_add(&sql_buf, &strings[STR_ESC]);
if (!DB_FLD_LAST(fld[i + 1])) rv |= sb_add(&sql_buf, set_str(&tmpstr, ", "));
}
if (rv) {
goto err;
}
if (!DB_FLD_EMPTY(cmd->match)) {
rv |= sb_add(&sql_buf, &strings[STR_WHERE]);
for(i = 0, fld = cmd->match; !DB_FLD_LAST(fld[i]); i++) {
rv |= sb_add(&sql_buf, set_str(&tmpstr, fld[i].name));
switch(fld[i].op) {
case DB_EQ: rv |= sb_add(&sql_buf, &strings[STR_OP_EQ]); break;
case DB_NE: rv |= sb_add(&sql_buf, &strings[STR_OP_NE]); break;
case DB_LT: rv |= sb_add(&sql_buf, &strings[STR_OP_LT]); break;
case DB_GT: rv |= sb_add(&sql_buf, &strings[STR_OP_GT]); break;
case DB_LEQ: rv |= sb_add(&sql_buf, &strings[STR_OP_LEQ]); break;
case DB_GEQ: rv |= sb_add(&sql_buf, &strings[STR_OP_GEQ]); break;
}
rv |= sb_add(&sql_buf, &strings[STR_ESC]);
if (!DB_FLD_LAST(fld[i + 1])) rv |= sb_add(&sql_buf, &strings[STR_AND]);
}
}
rv |= sb_add(&sql_buf, set_str(&tmpstr, "\0"));
if (rv) {
goto err;
}
sql_cmd->s = sql_buf.s;
sql_cmd->len = sql_buf.len;
return 0;
err:
if (sql_buf.s) pkg_free(sql_buf.s);
return -1;
}
static inline void update_field(MYSQL_BIND *param, db_fld_t* fld)
{
struct my_fld* fp; /* field payload */
struct tm* t;
fp = DB_GET_PAYLOAD(fld);
#ifndef MYSQL_FAKE_NULL
fp->is_null = fld->flags & DB_NULL;
if (fp->is_null) return;
#else
if (fld->flags & DB_NULL) {
switch(fld->type) {
case DB_STR:
case DB_CSTR:
param->buffer = FAKE_NULL_STR.s;
fp->length = FAKE_NULL_STR.len;
break;
case DB_INT:
*(int*)param->buffer = FAKE_NULL_INT;
break;
case DB_BLOB:
case DB_DATETIME:
case DB_NONE:
case DB_FLOAT:
case DB_DOUBLE:
case DB_BITMAP:
/* we don't have fake null value for these types */
fp->is_null = DB_NULL;
break;
}
return;
}
#endif
switch(fld->type) {
case DB_STR:
param->buffer = fld->v.lstr.s;
fp->length = fld->v.lstr.len;
break;
case DB_BLOB:
param->buffer = fld->v.blob.s;
fp->length = fld->v.blob.len;
break;
case DB_CSTR:
param->buffer = (char*)fld->v.cstr;
fp->length = strlen(fld->v.cstr);
break;
case DB_DATETIME:
t = gmtime(&fld->v.time);
fp->time.second = t->tm_sec;
fp->time.minute = t->tm_min;
fp->time.hour = t->tm_hour;
fp->time.day = t->tm_mday;
fp->time.month = t->tm_mon + 1;
fp->time.year = t->tm_year + 1900;
break;
case DB_NONE:
case DB_INT:
case DB_FLOAT:
case DB_DOUBLE:
case DB_BITMAP:
/* No need to do anything for these types */
break;
}
}
/**
* Update values of MySQL bound parameters with values from
* the DB API.
* @param cmd Command structure which contains pointers to MYSQL_STMT and parameters values
* @see bind_mysql_params
*/
static inline void set_mysql_params(db_cmd_t* cmd)
{
struct my_cmd* mcmd;
int i;
mcmd = DB_GET_PAYLOAD(cmd);
/* FIXME: We are updating internals of the prepared statement here,
* this is probably not nice but I could not find another way of
* updating the pointer to the buffer without the need to run
* mysql_stmt_bind_param again (which would be innefficient)
*/
for(i = 0; i < cmd->vals_count; i++) {
update_field(mcmd->st->params + i, cmd->vals + i);
}
for(i = 0; i < cmd->match_count; i++) {
update_field(mcmd->st->params + cmd->vals_count + i, cmd->match + i);
}
}
static inline int update_result(db_fld_t* result, MYSQL_STMT* st)
{
int i;
struct my_fld* rp; /* Payload of the current field in result */
struct tm t;
/* Iterate through all the fields returned by MySQL and convert
* them to DB API representation if necessary
*/
for(i = 0; i < st->field_count; i++) {
rp = DB_GET_PAYLOAD(result + i);
if (rp->is_null) {
result[i].flags |= DB_NULL;
continue;
} else {
result[i].flags &= ~DB_NULL;
}
switch(result[i].type) {
case DB_STR:
result[i].v.lstr.len = rp->length;
#ifdef MYSQL_FAKE_NULL
if (STR_EQ(FAKE_NULL_STR,result[i].v.lstr)) {
result[i].flags |= DB_NULL;
}
#endif
break;
case DB_BLOB:
result[i].v.blob.len = rp->length;
break;
case DB_CSTR:
if (rp->length < STR_BUF_SIZE) {
result[i].v.cstr[rp->length] = '\0';
} else {
/* Truncated field but rp->length contains full size,
* zero terminated the last byte in the buffer
*/
result[i].v.cstr[STR_BUF_SIZE - 1] = '\0';
}
#ifdef MYSQL_FAKE_NULL
if (strcmp(FAKE_NULL_STR.s,result[i].v.cstr)==0) {
result[i].flags |= DB_NULL;
}
#endif
break;
case DB_DATETIME:
memset(&t, '\0', sizeof(struct tm));
t.tm_sec = rp->time.second;
t.tm_min = rp->time.minute;
t.tm_hour = rp->time.hour;
t.tm_mday = rp->time.day;
t.tm_mon = rp->time.month - 1;
t.tm_year = rp->time.year - 1900;
/* Daylight saving information got lost in the database
* so let timegm to guess it. This eliminates the bug when
* contacts reloaded from the database have different time
* of expiration by one hour when daylight saving is used
*/
t.tm_isdst = -1;
#ifdef HAVE_TIMEGM
result[i].v.time = timegm(&t);
#else
result[i].v.time = _timegm(&t);
#endif /* HAVE_TIMEGM */
break;
case DB_INT:
#ifdef MYSQL_FAKE_NULL
if (FAKE_NULL_INT==result[i].v.int4) {
result[i].flags |= DB_NULL;
}
break;
#endif
case DB_NONE:
case DB_FLOAT:
case DB_DOUBLE:
case DB_BITMAP:
/* No need to do anything for these types */
break;
}
}
return 0;
}
