static void em_1000geodpos(struct gps_state *gps)
{
double speed;
int bearing, solinv;
solinv = INT16(&packet[WD(10)]) & 0x5;
if (solinv) gps->fix &= ~0x3; /* do we know whether it is a 2D or 3D fix? */
else gps->fix |= 0x3;
gps->lat = INT32(&packet[WD(27)]) * 1.0e-8;
gps->lon = INT32(&packet[WD(29)]) * 1.0e-8;
gps->alt = INT32(&packet[WD(31)]) * 1.0e-2;
speed = ((unsigned int)INT32(&packet[WD(34)])) * 1.0e-2;
bearing = radtodeg(INT16(&packet[WD(36)]) * 1.0e-3);
if (bearing < 0) bearing += 2 * M_PI;
gps->bearing = radtodeg(bearing);
gps->spd_up = 0.0;
gps->spd_east = sin(bearing) * speed;
gps->spd_north = cos(bearing) * speed;
gps->time = conv_date(INT16(&packet[WD(21)]),
INT16(&packet[WD(20)]), INT16(&packet[WD(19)])) +
INT16(&packet[WD(24)])+(60*INT16(&packet[WD(23)])) +
(3600*INT16(&packet[WD(22)]));
gps->updated |= GPS_STATE_FIX | GPS_STATE_COORD | GPS_STATE_BEARING | GPS_STATE_SPEED;
}
static void
smmapclick (GtkWidget *w, GdkEventButton *event)
{
if (event->button == 1)
{
smvector[selectedsmvector].x = event->x / (double)OMWIDTH;
smvector[selectedsmvector].y = event->y / (double)OMHEIGHT;
}
else if (event->button == 2)
{
if (numsmvect + 1 == MAXSIZEVECT)
return;
smvector[numsmvect].x = event->x / (double)OMWIDTH;
smvector[numsmvect].y = event->y / (double)OMHEIGHT;
smvector[numsmvect].siz = 0.0;
smvector[numsmvect].str = 1.0;
selectedsmvector = numsmvect;
numsmvect++;
updatesmsliders ();
}
#if 0
else if (event->button == 3)
{
double d;
d = atan2 (OMWIDTH * smvector[selectedsmvector].x - event->x,
OMHEIGHT * smvector[selectedsmvector].y - event->y);
smvector[selectedsmvector].dir = radtodeg (d);
updatesmsliders ();
*/
}
void controlpanel_odometry() {
OdomData odom;
while(true) {
char ch = controlpanel_promptChar("Odometry");
switch (ch) {
case 'p':
odom = odometry_getPos();
printf_P(PSTR("X: %f, Y: %f, Heading (deg): %f\n"), odom.x_pos, odom.y_pos, radtodeg(odom.heading));
break;
case 'q':
return;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" p - Current Position Data\n"
" ' '- Stop\n"
" q - Quit";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
void directionalhdl::update()
{
if (model != NULL)
{
glPushMatrix();
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
mat4f normal;
glGetFloatv(GL_TRANSPOSE_MODELVIEW_MATRIX, (float*)normal.data);
normal = transpose(inverse(normal));
direction = normal*vec4f(0.0, 0.0, -1.0, 0.0);
glPopMatrix();
}
}
void pointhdl::update()
{
if (model != NULL)
{
glPushMatrix();
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
mat4f modelview;
glGetFloatv(GL_TRANSPOSE_MODELVIEW_MATRIX, (float*)modelview.data);
vec4f p = modelview*vec4f(0.0, 0.0, 0.0, 1.0);
position = p(0,3)/p[3];
glPopMatrix();
}
}
void draw(const Ball& b) {
glColor4fv(color::ball_color);
glColor4fv(color::WHITE);
glPushMatrix();
glTranslatef(b.x(), b.y(), 0.1);
glRotatef(radtodeg(atan2(b.speed.y,b.speed.x)),0,0,1);
image::ball_image->activate();
draw_circle(0, 0, b.r());
image::ball_image->deactivate();
glPopMatrix();
}
void spothdl::update()
{
if (model != NULL)
{
glPushMatrix();
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
mat4f normal, modelview;
glGetFloatv(GL_TRANSPOSE_MODELVIEW_MATRIX, (float*)modelview.data);
normal = transpose(inverse(modelview));
vec4f p = modelview*vec4f(0.0, 0.0, 0.0, 1.0);
position = p(0,3)/p[3];
direction = normal*vec4f(0.0, 0.0, -1.0, 0.0);
glPopMatrix();
}
}
void directionalhdl::update()
{
/* TODO Assignment 2: Update the direction of the light using the orientation of the attached model.
* The easiest thing is to do translations and rotations like you were going to render the object, and
* then just multiply some initial direction vector by the normal matrix.
*/
if(model)
{
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
GLdouble modelview[16];
glGetDoublev( GL_TRANSPOSE_MODELVIEW_MATRIX, modelview );
mat4f mdvl;
for(int i = 0; i < 16; i++){
int row = i / 4;
int col = i % 4;
mdvl[row][col] = modelview[i];
}
mat4f normal = transpose(inverse(mdvl));
direction = normal * vec4f(0.0, 0.0, -1.0, 0.0);
glRotatef(-radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
glRotatef(-radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(-radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glTranslatef(-model->position[0], -model->position[1], -model->position[2]);
}
}
void spothdl::update()
{
/* TODO Assignment 2: Update both the direction and position of the light using the position and orientation
* of the attached model. See above.
*/
if(model)
{
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
GLdouble modelview[16];
glGetDoublev( GL_TRANSPOSE_MODELVIEW_MATRIX, modelview );
mat4f mdvl;
for(int i = 0; i < 16; i++){
int row = i / 4;
int col = i % 4;
mdvl[row][col] = modelview[i];
}
mat4f normal = transpose(inverse(mdvl));
direction = normal * vec4f(0.0, 0.0, -1.0, 0.0);
vec4f p = mdvl * vec4f(0.0, 0.0, 0.0, 1.0);
position = p(0,3)/p[3];
glRotatef(-radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
glRotatef(-radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(-radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glTranslatef(-model->position[0], -model->position[1], -model->position[2]);
}
}
void pointhdl::update()
{
/* TODO Assignment 2: Update the position of the light using the position of the attached model.
* The easiest thing is to do translations and rotations like you were going to render the object, and
* then just multiply the origin by the modelview matrix.
*/
if(model)
{
glTranslatef(model->position[0], model->position[1], model->position[2]);
glRotatef(radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glRotatef(radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
GLdouble modelview[16];
glGetDoublev( GL_TRANSPOSE_MODELVIEW_MATRIX, modelview );
mat4f mdvl;
for(int i = 0; i < 16; i++){
int row = i / 4;
int col = i % 4;
mdvl[row][col] = modelview[i];
}
vec4f p = mdvl * vec4f(0.0, 0.0, 0.0, 1.0);
position = p(0,3)/p[3];
glRotatef(-radtodeg(model->orientation[2]), 0.0, 0.0, 1.0);
glRotatef(-radtodeg(model->orientation[1]), 0.0, 1.0, 0.0);
glRotatef(-radtodeg(model->orientation[0]), 1.0, 0.0, 0.0);
glTranslatef(-model->position[0], -model->position[1], -model->position[2]);
}
}
void *
cx_atan(void *data, short int type, int length, int *newlength, short int *newtype, ...)
{
double *d;
d = alloc_d(length);
*newtype = VF_REAL;
*newlength = length;
if (type == VF_COMPLEX) {
complex *cc = (complex *) data;
int i;
for (i = 0; i < length; i++)
d[i] = radtodeg(atan(realpart(&cc[i])));
} else {
double *dd = (double *) data;
int i;
for (i = 0; i < length; i++)
d[i] = radtodeg(atan(dd[i]));
}
return ((void *) d);
}
static double calc_distance(const double lat1, const double lon1, const double lat2, const double lon2)
{
double theta = lon1 - lon2;
double dist = sin(degtorad(lat1)) * sin(degtorad(lat2))
+ cos(degtorad(lat1)) * cos(degtorad(lat2)) * cos(degtorad(theta));
dist = acos(dist);
dist = radtodeg(dist);
dist = dist * 60 * 1.1515;
dist = dist * 1.609344; /* mile to km */
dist = dist * 1000.00; /* km to m */
return dist;
}
void draw_line(const Vec& start, const Vec& end, const float width=2) {
glPushMatrix();
glTranslatef(start.x, start.y, 0);
Vec relv = end - start;
float phi = radtodeg(atan2(relv.y, relv.x));
glRotatef(phi,0,0,1);
glBegin(GL_QUADS);
glTexCoord2f(0,1); glVertex2f(0, -width);
glTexCoord2f(0,0); glVertex2f(0, width);
float abs = relv.size();
glTexCoord2f(abs/2/width,0); glVertex2f(abs, width);
glTexCoord2f(abs/2/width,1); glVertex2f(abs, -width);
glEnd();
glPopMatrix();
}
void *
cx_group_delay(void *data, short int type, int length, int *newlength, short int *newtype, struct plot *pl, struct plot *newpl, int grouping)
{
complex *cc = (complex *) data;
double *v_phase = alloc_d(length);
double *datos,adjust_final;
double *group_delay = alloc_d(length);
int i;
/* char *datos_aux; */
/* Check to see if we have the frequency vector for the derivative */
if (!eq(pl->pl_scale->v_name, "frequency"))
{
fprintf(cp_err, "Internal error: cx_group_delay: need frequency based complex vector.\n");
return (NULL);
}
if (type == VF_COMPLEX)
for (i = 0; i < length; i++)
{
v_phase[i] = radtodeg(cph(&cc[i]));
}
else
{
fprintf(cp_err, "Signal must be complex to calculate group delay\n");
return (NULL);
}
type = VF_REAL;
/* datos_aux = (char *)cx_deriv((char *)v_phase, type, length, newlength, newtype, pl, newpl, grouping);
* datos = (double *) datos_aux;
*/
datos = (double *)cx_deriv((char *)v_phase, type, length, newlength, newtype, pl, newpl, grouping);
/* With this global variable I will change how to obtain the group delay because
* it is defined as:
*
* gd()=-dphase[rad]/dw[rad/s]
*
* if you have degrees in phase and frequency in Hz, must be taken into account
*
* gd()=-dphase[deg]/df[Hz]/360
* gd()=-dphase[rad]/df[Hz]/(2*pi)
*/
if(cx_degrees)
{
adjust_final=1.0/360;
}
else
{
adjust_final=1.0/(2*M_PI);
}
for (i = 0; i < length; i++)
{
group_delay[i] = -datos[i]*adjust_final;
}
/* Adjust to Real because the result is Real */
*newtype = VF_REAL;
/* Set the type of Vector to "Time" because the speed of group units' s'
* The different types of vectors are INCLUDE \ Fte_cons.h
*/
pl->pl_dvecs->v_type= SV_TIME;
return ((char *) group_delay);
}
void controlpanel_sensor() {
while (true) {
switch (controlpanel_promptChar("Sensor")) {
case 'a': // Prints raw adc sensor values pins 0 - 7
for (int i=0; i<8; i++)
printf_P(PSTR("%4d "), adc_sampleAverage(i, 10));
putchar('\n');
break;
case 'r': // Prints all rangefinder readings in centimeters
printf_P(PSTR("Side Left: %5f, Front Left: %5f, Front Right: %5f, Side Right: %5f\n"), adc_sampleRangeFinder(ADC_SIDE_LEFT_RANGE), adc_sampleRangeFinder(ADC_FRONT_LEFT_RANGE), adc_sampleRangeFinder(ADC_FRONT_RIGHT_RANGE), adc_sampleRangeFinder(ADC_SIDE_RIGHT_RANGE));
break;
case 'l': { // Prints out raw linesensor data
uint16_t linebuf[linesensor_count];
linesensor_read(linebuf);
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%-5u "), linebuf[i]);
putchar('\n');
break;
}
case 'L': { // Prints out full crunched linesensor data
debug_resetTimer();
LineFollowResults results = linefollow_readSensor();
uint16_t time = debug_getTimer();
printf_P(PSTR("Light:\t"));
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%2.2f\t"), results.light[i]);
putchar('\n');
printf_P(PSTR("Thresh:\t"));
for (int i=0; i<linesensor_count; i++)
printf_P(PSTR("%d\t"), results.thresh[i]);
putchar('\n');
printf_P(PSTR("Center:\t%f\n"), results.center);
printf_P(PSTR("Turn:\t")); linefollow_printTurn(results.turn); putchar('\n');
printf_P(PSTR("Feat:\t")); linefollow_printFeature(results.feature); putchar('\n');
printf_P(PSTR("Time:\t%uus\n"), time);
break;
}
case 't': {
float thresh;
printf_P(PSTR("Current threshold: %f\n"), linefollow_getThresh());
if (controlpanel_prompt("Threshold", "%f", &thresh) == 1) {
printf_P(PSTR("Threshold changed to %f\n"), thresh);
linefollow_setThresh(thresh);
} else {
printf_P(PSTR("Cancelled.\n"));
}
break;
}
case 'b':
printf_P(PSTR("Battery voltage: %.2f\n"), adc_getBattery());
break;
case 'm': { // Prints out raw magnetometer data
MagReading reading = mag_getReading();
printf_P(PSTR("mag: %5d %5d %5d\n"), reading.x, reading.y, reading.z);
break;
}
case 'M': { // Prints out magnetometer calibrated heading
float heading = magfollow_getHeading();
heading = radtodeg(heading);
printf_P(PSTR("Mag Heading: %f\n"), heading);
break;
}
case 'H': { // Sets current heading of robot to prompted heading from user
float newheading;
controlpanel_prompt("Heading", "%f", &newheading);
magfollow_setHeading(degtorad(newheading));
break;
}
case 'q':
return;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Sensor commands\n"
" a - Dump analog port A\n"
" r - Rangefinder control panel\n"
" l - Raw line sensor readings\n"
" L - Processed line sensor readings\n"
" b - Battery voltage (approx)\n"
" m - Magnetometer\n"
" M - Magnetometer Heading\n"
" H - Set Magnetometer Heading\n"
" q - Back";
puts_P(msg);
break;
}
}
}
void controlpanel_autonomy() {
float follow = 0;
char input = ' ';
OdomData odom;
while (true) {
char ch = controlpanel_promptChar("Autonomy");
switch (ch) {
case 'G': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
obstacleAvoidance_setEnabled(true);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'g': {
float x_des, y_des, vel;
odom = odometry_getPos();
printf_P(PSTR("Current Position, X (meters): %f, Y (meters): %f, Heading (deg): %f\n"), cmtom(odom.x_pos), cmtom(odom.y_pos), radtodeg(odom.heading));
controlpanel_prompt("Goto X (meters): ", "%f", &x_des);
controlpanel_prompt("Goto Y (meters): ", "%f", &y_des);
controlpanel_prompt("At, Vel (cm/s): ", "%f", &vel);
goto_pos(mtocm(x_des), mtocm(y_des), vel);
break;
}
case 'e':
if (goto_getEnabled()) {
printf_P(PSTR("Goto: enabled, "));
} else {
printf_P(PSTR("Goto: disabled, "));
}
if (magfollow_enabled()) {
printf_P(PSTR("Magfollow: enabled, "));
} else {
printf_P(PSTR("Magfollow: disabled, "));
}
if (obstacleAvoidance_getEnabled()) {
printf_P(PSTR("Obstacle Avoidance: enabled.\n"));
} else {
printf_P(PSTR("Obstacle Avoidance: disabled.\n"));
}
break;
case 's': { // Sets current heading of robot to prompted heading from user
float newheading;
controlpanel_prompt("Heading (deg)", "%f", &newheading);
magfollow_setHeading(degtorad(newheading));
break;
}
case 'h': { // Prints out magnetometer calibrated heading
float heading = magfollow_getHeading();
heading = radtodeg(heading);
printf_P(PSTR("Mag Heading (deg): %f\n"), heading);
break;
}
case 'w': {
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Follow at Heading (deg)", "%f", &follow);
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
break;
}
case 'a':
follow = follow + 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 'd':
follow = follow - 5;
if (magfollow_enabled()) {
magfollow_start(setSpeed, anglewrap(degtorad(follow)));
printf_P(PSTR("Following at (deg): %f\n"), follow);
} else {
printf_P(PSTR("Not following, but heading set to (deg): %f\n"), follow);
}
break;
case 't':
printf_P(PSTR("Currently Facing (deg): %f\n"), radtodeg(magfollow_getHeading()));
controlpanel_prompt("Turn to Heading (deg)", "%f", &follow);
follow = degtorad(follow);
printf_P(PSTR("Currently at (deg): %f, Turning to (deg): %f\n"), radtodeg(magfollow_getHeading()), radtodeg(follow));
magfollow_turn(setSpeed, anglewrap(follow));
break;
case 'o':
obstacleAvoidance_setEnabled(false);
printf_P(PSTR("Obstacle Avoidance Disabled.\n"));
break;
case 'O':
obstacleAvoidance_setEnabled(true);
printf_P(PSTR("Obstacle Avoidance Enabled!\n"));
break;
case 'c': {
printf_P(PSTR("Beginning auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
calibrate_stationary();
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'C': {
printf_P(PSTR("Beginning Competition Routine!\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
Field field = calibrate_competition();
float vel;
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
printf_P(PSTR("Running Spiral-In Course!\n"));
overlap_run(field.xi, field.yi, field.xj, field.yj, field.xk, field.yk, field.xl, field.yl, vel);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
break;
}
case 'i': {
printf_P(PSTR("Beginning competition auto-cal!\nTurn robot to face 0 Degrees in field.\n"));
input = controlpanel_promptChar("Press 'Enter' to begin or any other key to cancel.");
if (input == 10) {
printf_P(PSTR("Calibrating...\n"));
magfollow_setOffset(0);
} else {
printf_P(PSTR("Auto-cal Cancelled.\n"));
}
float xi, yi;
float xj, yj;
float xk, yk;
float xl, yl;
float vel;
controlpanel_prompt("Xi (meters): ", "%f", &xi);
controlpanel_prompt("Yi (meters): ", "%f", &yi);
controlpanel_prompt("Xj (meters): ", "%f", &xj);
controlpanel_prompt("Yj (meters): ", "%f", &yj);
controlpanel_prompt("Xk (meters): ", "%f", &xk);
controlpanel_prompt("Yk (meters): ", "%f", &yk);
controlpanel_prompt("Xl (meters): ", "%f", &xl);
controlpanel_prompt("Yl (meters): ", "%f", &yl);
controlpanel_prompt("Velocity (cm/s): ", "%f", &vel);
spiralIn_run(xi, yi, xj, yj, xk, yk, xl, yl, vel);
break;
}
case 'f':
debug_halt("testing");
break;
case ' ':
magfollow_stop();
obstacleAvoidance_setEnabled(false);
goto_setEnabled(false);
break;
case 'q':
magfollow_stop();
return;
case '?':
static const char msg[] PROGMEM =
"Control Panels:\n"
" G - Goto Coordinate w/ Obstacle Avoidance\n"
" g - Goto Coordinate\n"
" e - Print states of enables\n"
" s - Set Heading\n"
" h - Current Heading\n"
" w - Magfollow\n"
" a - Shift following left\n"
" d - Shift following right\n"
" t - MagTurn\n"
" O/o - Enable/Disable Obstacle Avoidance\n"
" c - Auto-Calibration Routine\n"
" C - Do Competition Routine\n"
" i - Run Spiral-In competition\n"
" f - Halt\n"
" ' ' - Stop\n"
" q - Quit";
puts_P(msg);
break;
default:
puts_P(unknown_str);
break;
}
}
}
float Vector2Df::getAngle() const {
return radtodeg(acos(x));
}
