void track_puck() {
float KP = 1;
float KD = 0;
if (!TRACK_PUCK) return;
int goalie_theta_est = estimate_track_puck_theta();
if (first_move) {
prev_goalie_theta_est = goalie_theta_est;
first_move = FALSE;
}
int speed = KP * goalie_theta_est - KD * (goalie_theta_est - prev_goalie_theta_est);
prev_goalie_theta_est = goalie_theta_est;
set_motor_duty_cycle(-speed, -speed);
m_usb_tx_string("--------------------TRACK PUCK --------------------\n");
print_raw_sensor_values();
print_sensor_values();
send_float("goalie_theta_est", goalie_theta_est);
send_float("speed", speed);
send_float("tgt_duty_cycle_R", -speed);
send_float("tgt_duty_cycle_L", -speed);
}
void send_set_tile_quality(unsigned int i, const float q[2])
{
send_event(EVENT_SET_TILE_QUALITY);
send_index(i);
send_float((tile[i].quality[0] = q[0]));
send_float((tile[i].quality[1] = q[1]));
}
void send_set_camera_range(int i, float n, float f)
{
struct camera *c = get_camera(i);
send_event(EVENT_SET_CAMERA_RANGE);
send_index(i);
send_float((c->n = n));
send_float((c->f = f));
}
int parse_line(char * in_line){
int i = 0;
int initial_length = strlen(in_line);
printf("chars parsed: %d\n", initial_length);
char * token = strtok_single(in_line, " ");
/* walk through other tokens */
//The first call you use a char array which has the elements you want parsed.
//The second time you call it you pass it NULL as the first parameter to tell function
//to resume from the last spot in the string. Once the first call is made your char
//array receives the parsed string. If you don't put NULL you would lose your place
//and effectivly the last part of your string.
if (initial_length > 10){
if (initial_length >= 200){
//grid population
send_byte((uint8_t)*(token));
printf("%s ", token);
while(token){
i++;
token = strtok_single(NULL, " ");
if (i < 100) {
send_byte((uint8_t)*(token));
printf("%s ",token);
}
usleep(200000);
}
printf("\n");
}
else{
//Station line
//printf("callsign:%s\n", token);
send_string(token);
if (strlen(token) < 8)
{
int k=0;
for (k=0; k< 8-strlen(token); k++)
{
send_string(" ");
}
}
while(token){
i++;
token = strtok_single(NULL, " ");
if (i < 6) send_float(token);
//if (i < 6) printf("float:%s ",token);
usleep(200000);
}
}
}
else{
send_float(token);
//printf("num_stations: %s\n",token);
}
printf("\n");
return 0;
}
void send_set_sprite_range(int i, float s0, float s1, float t0, float t1)
{
struct sprite *s = get_sprite(i);
send_event(EVENT_SET_SPRITE_RANGE);
send_index(i);
send_float((s->s0 = s0));
send_float((s->s1 = s1));
send_float((s->t0 = t0));
send_float((s->t1 = t1));
}
void send_set_tile_linescrn(unsigned int i, float p, float a,
float t, float s, float c)
{
send_event(EVENT_SET_TILE_LINESCRN);
send_index(i);
send_float((tile[i].varrier_pitch = p));
send_float((tile[i].varrier_angle = a));
send_float((tile[i].varrier_thick = t));
send_float((tile[i].varrier_shift = s));
send_float((tile[i].varrier_cycle = c));
}
void send_set_camera_image(int i, int j, float l, float r, float b, float t)
{
struct camera *c = get_camera(i);
send_event(EVENT_SET_CAMERA_IMAGE);
send_index(i);
send_index((c->image = j));
send_float((c->l = l));
send_float((c->r = r));
send_float((c->b = b));
send_float((c->t = t));
}
int
sock_send_float(int purpose, double num)
{
if (accept_if_needed(purpose) != -1)
return send_float(purpose_table[purpose], num);
return -1;
}
void send_set_brush_line_width(int i, float w)
{
struct brush *b = get_brush(i);
send_event(EVENT_SET_BRUSH_LINE_WIDTH);
send_index(i);
send_float((b->line_width = w));
}
void send_set_galaxy_magnitude(int i, float m)
{
send_event(EVENT_SET_GALAXY_MAGNITUDE);
send_index(i);
send_float(m);
get_galaxy(i)->magnitude = m;
}
int
send_floats(Sock *sock, double *vals, int num)
{
int i;
for(i=0; i<num; i++)
if (send_float(sock, *vals++) == -1)
return -1;
return 0;
}
void send_set_tile_position(unsigned int i, const float o[3],
const float r[3],
const float u[3])
{
send_event(EVENT_SET_TILE_POSITION);
send_index(i);
send_float((tile[i].o[0] = o[0]));
send_float((tile[i].o[1] = o[1]));
send_float((tile[i].o[2] = o[2]));
send_float((tile[i].r[0] = r[0]));
send_float((tile[i].r[1] = r[1]));
send_float((tile[i].r[2] = r[2]));
send_float((tile[i].u[0] = u[0]));
send_float((tile[i].u[1] = u[1]));
send_float((tile[i].u[2] = u[2]));
}
int send_create_camera(int t)
{
int i;
if ((i = new_camera()) >= 0)
{
struct camera *c = get_camera(i);
c->count = 1;
c->type = t;
c->n = (t == CAMERA_ORTHO) ? -1000.0f : 0.1f;
c->f = (t == CAMERA_ORTHO) ? 1000.0f : 1000.0f;
c->frame = 0;
c->depth = 0;
c->image = 0;
c->view_basis[0][0] = 1.0f;
c->view_basis[0][1] = 0.0f;
c->view_basis[0][2] = 0.0f;
c->view_basis[1][0] = 0.0f;
c->view_basis[1][1] = 1.0f;
c->view_basis[1][2] = 0.0f;
c->view_basis[2][0] = 0.0f;
c->view_basis[2][1] = 0.0f;
c->view_basis[2][2] = 1.0f;
send_event(EVENT_CREATE_CAMERA);
send_index(t);
send_float(c->n);
send_float(c->f);
return send_create_entity(TYPE_CAMERA, i);
}
return -1;
}
void send_set_background(const float c0[3], const float c1[3])
{
send_event(EVENT_SET_BACKGROUND);
send_float((color0[0] = c0[0]));
send_float((color0[1] = c0[1]));
send_float((color0[2] = c0[2]));
send_float((color1[0] = c1[0]));
send_float((color1[1] = c1[1]));
send_float((color1[2] = c1[2]));
}
void send_set_camera_stereo(int i, const float L[3],
const float R[3], int mode)
{
struct camera *c = get_camera(i);
send_event(EVENT_SET_CAMERA_STEREO);
send_index(i);
send_index((c->mode = mode));
send_float((c->eye_offset[0][0] = L[0]));
send_float((c->eye_offset[0][1] = L[1]));
send_float((c->eye_offset[0][2] = L[2]));
send_float((c->eye_offset[1][0] = R[0]));
send_float((c->eye_offset[1][1] = R[1]));
send_float((c->eye_offset[1][2] = R[2]));
}
void send_set_camera_offset(int i, const float p[3], const float M[16])
{
struct camera *c = get_camera(i);
send_event(EVENT_SET_CAMERA_OFFSET);
send_index(i);
send_float((c->pos_offset[0] = p[0]));
send_float((c->pos_offset[1] = p[1]));
send_float((c->pos_offset[2] = p[2]));
send_float((c->view_basis[0][0] = M[0]));
send_float((c->view_basis[0][1] = M[1]));
send_float((c->view_basis[0][2] = M[2]));
send_float((c->view_basis[1][0] = M[4]));
send_float((c->view_basis[1][1] = M[5]));
send_float((c->view_basis[1][2] = M[6]));
send_float((c->view_basis[2][0] = M[8]));
send_float((c->view_basis[2][1] = M[9]));
send_float((c->view_basis[2][2] = M[10]));
}
