static double
geo_distance_internal(Point *pt1, Point *pt2)
{
double long1,
lat1,
long2,
lat2;
double longdiff;
double sino;
/* convert degrees to radians */
long1 = degtorad(pt1->x);
lat1 = degtorad(pt1->y);
long2 = degtorad(pt2->x);
lat2 = degtorad(pt2->y);
/* compute difference in longitudes - want < 180 degrees */
longdiff = fabs(long1 - long2);
if (longdiff > M_PI)
longdiff = TWO_PI - longdiff;
sino = sqrt(sin(fabs(lat1 - lat2) / 2.) * sin(fabs(lat1 - lat2) / 2.) +
cos(lat1) * cos(lat2) * sin(longdiff / 2.) * sin(longdiff / 2.));
if (sino > 1.)
sino = 1.;
return 2. * EARTH_RADIUS * asin(sino);
}
static void degvectoradvec(apivector * degvec) {
apivector tmp;
tmp.x=degtorad(degvec->x);
tmp.y=degtorad(degvec->y);
tmp.z=degtorad(degvec->z);
*degvec=tmp;
}
void nav_magGo(float heading_deg, float dist) {
if (dist > 0) {
magfollow_start(60, degtorad(heading_deg));
} else {
magfollow_start(-60, degtorad(heading_deg));
}
drive_waitDist(dist);
magfollow_stop();
}
int distance(double lat1, double lon1, double lat2, double lon2)
{
double minlat, maxlat, deltalong;
minlat = degtorad(lat1);
maxlat = degtorad(lat2);
deltalong = degtorad(lon2 - lon1);
return EARTH_RADIUS * acos(sin(minlat) * sin(maxlat) + cos(minlat) * cos(maxlat) * cos(deltalong)) * pMap->zoom;
}
static double *
d_tan(double *dd, int length)
{
double *d;
int i;
d = alloc_d(length);
for (i = 0; i < length; i++) {
rcheck(cos(degtorad(dd[i])) != 0, "tan");
d[i] = sin(degtorad(dd[i])) / cos(degtorad(dd[i]));
}
return 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;
}
Vector2Df Vector2Df::getRotated( float angle ) {
float rad = degtorad(angle);
float c = cos(rad);
float s = sin(rad);
return Vector2Df( x*c - y*s, x*s + y*c );
}
void draw_arrow(const Vec& start, const Vec& end, const float width=2, const float head=10, const float phi=30) {
draw_line(start, end, width);
// head line 1
float c = cos(degtorad(phi)), s = sin(degtorad(phi));
Vec hl1 = start - end, hl2;
hl1.normalize();
hl1 *= head;
hl2 = hl1;
hl1.matrix_multiply(c, -s, s, c);
hl2.matrix_multiply(c, s, -s, c);
hl1 += end;
hl2 += end;
draw_line(end, hl1, width);
draw_line(end, hl2, width);
draw_circle(start.x, start.y, width);
draw_circle(end.x, end.y, width);
draw_circle(hl1.x, hl1.y, width);
draw_circle(hl2.x, hl2.y, width);
}
std::pair<double,double> move_outside(std::pair<double,double> p1, std::pair<double,double> p2, SDL_Rect* r1, SDL_Rect* r2) {
double dist = distance(p1.first, p1.second, p2.first, p2.second);
double dir = direction_of(p2.first, p2.second, p1.first, p1.second);
double x = p2.first, y = p2.second;
int max_attempts = 10;
double delta = dist/((double)max_attempts);
int attempts = 0;
r1->x = p2.first; r1->y = p2.second;
while ((check_collision(r1, r2))&&(attempts++ < max_attempts)) {
x += sin(degtorad(dir)) * delta*dist;
y += -cos(degtorad(dir)) * delta*dist;
r1->x = x;
r1->y = y;
}
return std::make_pair(x, y);
}
/*
* draws a robot with a given size, using the various parameters
*(orientation,
* position, ..) from the robot struct
*/
void draw_robot(cairo_t* cr, struct robot* myRobot, double size)
{
double x1 = -70, y1 = -30,
y2 = 10,
x4 = 70,
x5 = 0, y5 = -100;
double px2 = -70, py2 = 100,
px3 = 70, py3 = 100,
px4 = 70, py4 = 10;
cairo_save(cr);
cairo_translate(cr, myRobot->x, myRobot->y);
cairo_scale(cr, size, size);
cairo_save(cr);
cairo_rotate(cr, degtorad(90 + myRobot->degree));
cairo_set_source_rgba(cr, myRobot->color[0], myRobot->color[1],
myRobot->color[2], 0.6);
cairo_set_line_width(cr, 2);
cairo_move_to(cr, x1, y1);
cairo_line_to(cr, x1, y2);
cairo_curve_to(cr, px2, py2, px3, py3, px4, py4);
cairo_line_to(cr, x4, y1);
cairo_line_to(cr, x5, y5);
cairo_close_path(cr);
cairo_set_fill_rule(cr, CAIRO_FILL_RULE_EVEN_ODD);
cairo_move_to(cr, 30, 0);
cairo_arc_negative(cr, 0, 0, 30, 0, -2 * M_PI);
cairo_fill_preserve(cr);
cairo_set_source_rgba(cr, 0.1, 0.7, 0.5, 0.5);
cairo_stroke(cr);
cairo_restore(cr); /* pop rotate */
draw_cannon(cr, degtorad(270 + myRobot->cannon_degree));
draw_radar(cr, degtorad(270 + myRobot->radar_degree));
cairo_restore(cr); /* pop translate/scale */
shot_animation(cr, size, degtorad(myRobot->cannon_degree),
&myRobot->cannon[0]);
shot_animation(cr, size, degtorad(myRobot->cannon_degree),
&myRobot->cannon[1]);
}
static complex *
c_tan(complex *cc, int length)
{
complex *c;
int i;
c = alloc_c(length);
for (i = 0; i < length; i++) {
double u, v;
rcheck(cos(degtorad(realpart(&cc[i]))) *
cosh(degtorad(imagpart(&cc[i]))), "tan");
rcheck(sin(degtorad(realpart(&cc[i]))) *
sinh(degtorad(imagpart(&cc[i]))), "tan");
u = degtorad(realpart(&cc[i]));
v = degtorad(imagpart(&cc[i]));
/* The Lattice C compiler won't take multi-line macros, and
* CMS won't take >80 column lines....
*/
#define xx1 sin(u) * cosh(v)
#define xx2 cos(u) * sinh(v)
#define xx3 cos(u) * cosh(v)
#define xx4 sin(u) * sinh(v)
cdiv(xx1, xx2, xx3, xx4, realpart(&c[i]), imagpart(&c[i]));
}
return c;
}
void AARectHitBox::setRotation(double frot)
{
rotation = frot;
degtorad(frot);
sf::Vector2f distvec;
sf::Vector2f rotdistvec;
for (unsigned int i = 0; i < 4; i++)
{
distvec = initCorners[i] - position;
rotdistvec.x = distvec.x * cos(-frot) + distvec.y * sin(-frot);
rotdistvec.y = -distvec.x * sin(-frot) + distvec.y * cos(-frot);
corners[i] = rotdistvec + position;
}
double maxX = position.x;
double minX = position.x;
double maxY = position.y;
double minY = position.y;
for (int i = 0; i < 4; i++)
{
if (corners[i].x > maxX)
{
maxX = corners[i].x;
}
if (corners[i].x < minX)
{
minX = corners[i].x;
}
if (corners[i].y > maxY)
{
maxY = corners[i].y;
}
if (corners[i].y < minY)
{
minY = corners[i].y;
}
}
corners[0] = sf::Vector2f(minX, minY);
corners[1] = sf::Vector2f(maxX, minY);
corners[2] = sf::Vector2f(maxX, maxY);
corners[3] = sf::Vector2f(minX, maxY);
height = maxY - minY;
width = maxX - minX;
}
static void taip_decode(struct gps_state *gps)
{
char buf[10];
double speed, b;
draw_activity(0);
if (strcmp(packet, "RTM") == 0) {
// datestamp = ???
} else if (strcmp(packet, "RPV") == 0) {
if (packet[32] == '0') return;
memcpy(buf, &packet[3], 5); buf[5] = '\0';
gps->time = strtol(buf, NULL, 10); // + datestamp;
memcpy(buf, &packet[8], 8); buf[8] = '\0';
gps->lat = degtorad((double)strtol(buf, NULL, 10) / 100000.0);
memcpy(buf, &packet[16], 9); buf[9] = '\0';
gps->lon = degtorad((double)strtol(buf, NULL, 10) / 100000.0);
gps->updated |= GPS_STATE_COORD;
memcpy(buf, &packet[28], 3); buf[3] = '\0';
gps->bearing = strtol(buf, NULL, 10);
gps->updated |= GPS_STATE_BEARING;
memcpy(buf, &packet[25], 3); buf[3] = '\0';
speed = (double)strtol(buf, NULL, 10) * 0.44704; // * 1609.344 / 3600
b = degtorad(gps->bearing);
gps->spd_east = sin(b) * speed;
gps->spd_north = cos(b) * speed;
gps->spd_up = 0.0;
gps->updated |= GPS_STATE_SPEED;
if (packet[31] == '9')
gps->bearing = -1;
}
}
Coord GetSegmentEnd(double Longueur, double Angle) {
Coord c = Coord(0,0);
double A = (int)(Angle + 360) % 360;
if ((int) A == 0) {
c.x = Longueur;
return c;
} else if ((int)A == 180) {
c.x= -Longueur;
return c;
} else if ((int)A == 90) { // Angle direction de plan normal alors que y evolue celons les conventions de coordonnée d'affichage
c.y = -Longueur;
return c;
} else if ((int)A == 270) {
c.y = Longueur;
return c;
}
c.x = (cos(degtorad(Angle)) * Longueur);
// Angle direction de plan normal alors que y evolue celons les conventions de coordonnée d'affichage
c.y = -(sin(degtorad(Angle)) * Longueur) ;
return c;
}
void ConvexHitBox::rotate(double frot)
{
rotation += frot;
degtorad(frot);
sf::Vector2f distVec;
sf::Vector2f rotDistVec;
for (unsigned int i = 0; i < vertecies.size(); i++)
{
distVec = vertecies[i] - position;
rotDistVec.x = distVec.x * cos(-frot) + distVec.y * sin(-frot);
rotDistVec.y = -distVec.x * sin(-frot) + distVec.y * cos(-frot);
vertecies[i] = rotDistVec + position;
}
}
void Graphics2D::setRotation(float rotation)
{
this->Rotation = rotation;
if(Rotation==0)
{
sinRad = 0;
cosRad = 1;
}
else
{
float radians = (float)degtorad(Rotation);
sinRad = sin(radians);
cosRad = cos(radians);
}
}
void Graphics2D::rotate(float rotation, float x1, float y1)
{
this->Rotation += rotation;
rotX = x1;
rotY = y1;
if(Rotation==0)
{
sinRad = 0;
cosRad = 1;
}
else
{
float radians = (float)degtorad(Rotation);
sinRad = sin(radians);
cosRad = cos(radians);
}
}
/*
* BEE::Sprite::draw_subimage() - Draw a given subimage of the sprite with the given attributes
* @x: the x-coordinate to draw the subimage at
* @y: the y-coordinate to draw the subimage at
* @current_subimage: the subimage of the sprite to draw
* @w: the width to scale the subimage to
* @h: the height to scale the subimage to
* @angle: the number of degrees to rotate the subimage clockwise
* @new_color: the color to paint the subimage in
* @flip: the type of flip to draw the subimage with
*/
int BEE::Sprite::draw_subimage(int x, int y, unsigned int current_subimage, int w, int h, double angle, RGBA new_color, SDL_RendererFlip flip) {
if (!is_loaded) { // Only attempt to draw the subimage if it has been loaded
if (!has_draw_failed) { // If the draw call hasn't failed before, issue a warning
game->messenger_send({"engine", "sprite"}, BEE_MESSAGE_WARNING, "Failed to draw sprite \"" + name + "\" because it is not loaded");
has_draw_failed = true; // Set the draw failure boolean
}
return 1; // Return 1 on failure
}
SDL_Rect drect = {x, y, 0, 0}; // Create a rectangle to define the position and dimensions of the destination render
// Determine the desired width and height of the render
if ((w >= 0)&&(h >= 0)) { // If the width and height are provided to the function, use them as is
drect.w = w;
drect.h = h;
} else { // Otherwise set the width and height to the same as the sprite, i.e. don't scale the render
drect.w = width;
drect.h = height;
if (subimage_amount > 1) {
drect.w = subimage_width;
}
}
if (game->options->renderer_type != BEE_RENDERER_SDL) {
// Get the full width of the sprite to be used for scaling
int rect_width = width;
if (subimage_amount > 1) {
rect_width = subimage_width;
}
// If the scaled width and height are not provided to the function, set them so that scaling will have no effect
if (w <= 0) {
w = rect_width;
}
if (h <= 0) {
h = height;
}
glBindVertexArray(vao); // Bind the VAO for the sprite
// Generate the partial transformation matrix (translation and scaling) for the subimage
glm::mat4 model = glm::translate(glm::mat4(1.0f), glm::vec3((float)drect.x, (float)drect.y, -0.5f)); // Translate the subimage the desired amount in the x- and y-planes, note that the z-coordinate is nonzero so that there is no z-fighting with backgrounds in 3D mode
model = glm::scale(model, glm::vec3((float)w/rect_width, (float)h/height, 1.0f)); // Scale the subimage in the x- and y-planes
glUniformMatrix4fv(game->model_location, 1, GL_FALSE, glm::value_ptr(model)); // Send the transformation matrix to the shader
// Generate the rotation matrix for the subimage
// This is not included in the above transformation matrix because it is faster to rotate everything in the geometry shader
if (angle != 0.0) {
glm::mat4 rotation = glm::translate(glm::mat4(1.0f), glm::vec3((float)rect_width*rotate_x, (float)height*rotate_y, 0.0f));
rotation = glm::rotate(rotation, (float)degtorad(angle), glm::vec3(0.0f, 0.0f, 1.0f)); // Rotate the subimage on the z-axis around the sprite's rotation origin at (rotate_x, rotate_y)
rotation = glm::translate(rotation, glm::vec3(-(float)rect_width*rotate_x, -(float)height*rotate_y, 0.0f));
glUniformMatrix4fv(game->rotation_location, 1, GL_FALSE, glm::value_ptr(rotation)); // Send the rotation matrix to the shader
}
// Bind the sprite texture
glUniform1i(game->texture_location, 0);
glBindTexture(GL_TEXTURE_2D, gl_texture);
// Colorize the sprite with the given color
float a = alpha; // Set the default alpha to be the sprite's general alpha value
if (new_color.a != 0) { // If the provided color has an alpha value, use that instead
a = (float)new_color.a/255.0f; // Normalize the alpha
}
glm::vec4 color = glm::vec4((float)new_color.r/255.0f, (float)new_color.g/255.0f, (float)new_color.b/255.0f, a); // Normalize the color values from 0.0 to 1.0
glUniform4fv(game->colorize_location, 1, glm::value_ptr(color)); // Send the color to the shader
// Determine the desired flip type
int f = 0; // The default behavior is to not flip
if (flip & SDL_FLIP_HORIZONTAL) {
f += 1;
}
if (flip & SDL_FLIP_VERTICAL) {
f += 2;
}
glUniform1i(game->flip_location, f); // Send the flip type to the shader
// Add the subimage to the list of lightables so that it can cast shadows
if (is_lightable) { // If the sprite is set as lightable
// Fill a lightable data struct with the position and vertices of the subimage
LightableData* l = new LightableData();
l->position = glm::vec4((float)drect.x, (float)drect.y, 0.0f, 0.0f);
l->mask.push_back(glm::vec4(0.0f, 0.0f, 0.0f, 0.0f));
l->mask.push_back(glm::vec4((float)rect_width, 0.0f, 0.0f, 0.0f));
l->mask.push_back(glm::vec4((float)rect_width, (float)height, 0.0f, 0.0f));
l->mask.push_back(glm::vec4(0.0f, (float)height, 0.0f, 0.0f));
game->get_current_room()->add_lightable(l); // Add the struct to the room's list of lightables
}
// Bind the texture coordinates of the current subimage
glEnableVertexAttribArray(game->fragment_location);
glBindBuffer(GL_ARRAY_BUFFER, vbo_texcoords[current_subimage]);
glVertexAttribPointer(
game->fragment_location,
2,
GL_FLOAT,
GL_FALSE,
0,
0
);
// Draw the triangles which form the rectangular subimage
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibo);
int size;
glGetBufferParameteriv(GL_ELEMENT_ARRAY_BUFFER, GL_BUFFER_SIZE, &size);
glDrawElements(GL_TRIANGLES, size/sizeof(GLushort), GL_UNSIGNED_SHORT, 0);
// Reset the shader state
glDisableVertexAttribArray(game->fragment_location); // Unbind the texture coordinates
glUniformMatrix4fv(game->model_location, 1, GL_FALSE, glm::value_ptr(glm::mat4(1.0f))); // Reset the partial transformation matrix
glUniformMatrix4fv(game->rotation_location, 1, GL_FALSE, glm::value_ptr(glm::mat4(1.0f))); // Reset the rotation matrix
glBindTexture(GL_TEXTURE_2D, 0); // Unbind the sprite texture
glUniform1i(game->flip_location, 0); // Reset the flip type
glBindVertexArray(0); // Unbind the VAO
} else {
if (game->is_on_screen(drect)) { // If the render will actually appear on screen
// Colorize the sprite with the given color
SDL_SetTextureColorMod(texture, new_color.r, new_color.g, new_color.b);
if (new_color.a == 0) { // If the given color doesn't have an alpha value, use the sprite's general value
SDL_SetTextureAlphaMod(texture, alpha*255);
} else { // Otherwise, use the value from the given color
SDL_SetTextureAlphaMod(texture, new_color.a);
}
// Reset the sprite's blend mode to the global mode
SDL_SetTextureBlendMode(texture, game->draw_get_blend());
SDL_Point r = {(int)(rotate_x*subimage_width), (int)(rotate_y*height)}; // Create a point to use as the rotation origin
// Render the subimage
if (!subimages.empty()) { // If the sprite has multiple subimages, render without further cropping
SDL_RenderCopyEx(game->renderer, texture, &subimages[current_subimage], &drect, angle, &r, flip);
} else { // Otherwise, render and crop as requested
SDL_RenderCopyEx(game->renderer, texture, &crop, &drect, angle, &r, flip);
}
}
}
// If the sprite has reached the end of its subimage cycle, set the animation boolean
if ((is_animated)&&(current_subimage == subimage_amount-1)) {
is_animated = false;
}
return 0; // Return 0 on success
}
void d3d_transform_set_rotation_z(gs_scalar angle)
{
enigma::model_matrix.init_rotate_z_transform(degtorad(-angle));
enigma::mv_matrix = enigma::view_matrix * enigma::model_matrix;
}
double approx_cosd(double deg)
{
return approx_cos(degtorad(deg));
}
double approx_sind(double deg)
{
return approx_sin(degtorad(deg));
}
double approx_tand(double deg)
{
return approx_tan(degtorad(deg));
}
double long_to_x(double longitude)
{
return EARTH_RADIUS * degtorad(longitude);
}
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;
}
}
}
double lat_to_y(double latitude)
{
return EARTH_RADIUS * log(tan(M_PI / 4 + degtorad(latitude) / 2));
}
