Пример #1
0
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;
}
Пример #2
0
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 ();
    */
  }
Пример #3
0
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;
		}
	}
}
Пример #4
0
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();
	}
}
Пример #5
0
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();
	}
}
Пример #6
0
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();
}
Пример #7
0
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();
	}
}
Пример #8
0
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]);
	}
}
Пример #9
0
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]);
	}
}
Пример #10
0
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]);
	}
}
Пример #11
0
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;
    }
Пример #13
0
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();
}
Пример #14
0
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);

}
Пример #15
0
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;
		}
	}
}
Пример #16
0
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;
		}
	}
}
Пример #17
0
float Vector2Df::getAngle() const {
    return radtodeg(acos(x));
}