void setMatrices()
{
// Set up the projection and model-view matrices
GLfloat aspectRatio = (GLfloat)window.GetWidth()/window.GetHeight();
GLfloat nearClip = 0.1f;
GLfloat farClip = 500.0f;
GLfloat fieldOfView = 45.0f; // Degrees
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(fieldOfView, aspectRatio, nearClip, farClip);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0.0, 0.0, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f);
glPushMatrix(); // set the light in the scene correctly
transNode(cathedralScene, cathedralScene->mRootNode);
GLfloat light0_position[] = { 0, 30, 0, 0 };
glLightfv( GL_LIGHT0, GL_POSITION, light0_position );
GLfloat light1_position[] = { 0, 11, 0, 0.0 };
glLightfv( GL_LIGHT1, GL_POSITION, light1_position );
glPopMatrix();
glRotatef(rad_to_deg(pitch), 1.0, 0.0, 0.0);
glRotatef(rad_to_deg(yaw), 0.0, 1.0, 0.0);
glTranslatef(position.x, position.y, position.z);
// We store the modelview matrix here, but it's actually the view matrix because we haven't done any model transforms yet
// static double viewMatrix[16];
GLfloat *viewMatrix = new GLfloat[16];
glGetFloatv(GL_MODELVIEW_MATRIX, viewMatrix);
GLint inverseViewMatrixUniform = glGetUniformLocation(envMapShader->programID(), "inverseViewMatrix");
glUniformMatrix4fv(inverseViewMatrixUniform, 1, true, viewMatrix);
delete [] viewMatrix;
}
/*
INPUT: Data from CAN msg which has been put into the float of the fXYZ structure
OUTPUT: Result stored in file scope variable: MagnetAngularPosition
DESCRIPTION: Calculates the dot product of the incoming measurement
against the reference vector for each of the x=0, y=0, and
z=0 planes.
*/
void magnet_angles( struct fXYZ* mRaw )
{
// Similar to accel_
// Normalize the Raw Magnet vector
// Compared to 1 earth magnet field strength
struct fXYZ ref = magnetReference;
/* Project the baseline vector and the current vector
onto each plane (x=0,y=0,z=0),
mRaw dot_product Ref = cos(angle);
Solve for the angle between them.
*/
// Compare to baseline vector, not the unit vectors!
ref = magnetReference;
ref.x = 0; // yz plane
float S=mRaw->x; mRaw->x=0;
MagnetAngularPosition.rx = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->x=S;
ref = magnetReference;
ref.y = 0; // xz plane
S=mRaw->y; mRaw->y=0;
MagnetAngularPosition.ry = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->y=S;
ref = magnetReference;
ref.z = 0; // xy plane
S=mRaw->z; mRaw->z=0;
MagnetAngularPosition.rz = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->z=S;
// Need to Deal with Quadrants!?
// sensor is responding somewhat correctly. X is length of board.
}
void RotatedDC::DrawText (const String& text, const RealPoint& pos, AColor color, int blur_radius, int boldness, double stretch_) {
if (text.empty()) return;
if (color.alpha == 0) return;
if (quality >= QUALITY_AA) {
RealRect r(pos, GetTextExtent(text));
RealRect r_ext = trRectToBB(r);
RealPoint pos2 = tr(pos);
stretch_ *= getStretch();
if (fabs(stretch_ - 1) > 1e-6) {
r.width *= stretch_;
RealRect r_ext2 = trRectToBB(r);
pos2.x += r_ext2.x - r_ext.x;
pos2.y += r_ext2.y - r_ext.y;
r_ext.x = r_ext2.x;
r_ext.y = r_ext2.y;
}
draw_resampled_text(dc, pos2, r_ext, stretch_, angle, color, text, blur_radius, boldness);
} else if (quality >= QUALITY_SUB_PIXEL) {
RealPoint p_ext = tr(pos)*text_scaling;
double usx,usy;
dc.GetUserScale(&usx, &usy);
dc.SetUserScale(usx/text_scaling, usy/text_scaling);
dc.SetTextForeground(color);
dc.DrawRotatedText(text, (int) p_ext.x, (int) p_ext.y, rad_to_deg(angle));
dc.SetUserScale(usx, usy);
} else {
RealPoint p_ext = tr(pos);
dc.SetTextForeground(color);
dc.DrawRotatedText(text, (int) p_ext.x, (int) p_ext.y, rad_to_deg(angle));
}
}
void Analyzing_MAVLink_Message_Handler::handle_decoded_message(uint64_t T, mavlink_ahrs2_t &msg) {
double lat = msg.lat/(double)10000000.0f;
double lng = msg.lng/(double)10000000.0f;
double alt = msg.altitude;
_vehicle->position_estimate("AHRS2")->set_lat(T, lat);
_vehicle->position_estimate("AHRS2")->set_lon(T, lng);
_vehicle->altitude_estimate("AHRS2")->set_alt(T, alt);
_vehicle->attitude_estimate("AHRS2")->set_roll(T, rad_to_deg(msg.roll));
_vehicle->attitude_estimate("AHRS2")->set_pitch(T, rad_to_deg(msg.pitch));
_vehicle->attitude_estimate("AHRS2")->set_yaw(T, rad_to_deg(msg.yaw));
// we fake up the vehicle origin by setting it whenever the
// vehicle moves from disarmed to armed
if (_vehicle->is_armed()) {
if (!set_origin_was_armed) {
_vehicle->set_origin_lat(lat);
_vehicle->set_origin_lon(lng);
_vehicle->set_origin_altitude(alt);
set_origin_was_armed = true;
}
} else {
_vehicle->set_origin_lat(0);
_vehicle->set_origin_lon(0);
_vehicle->set_origin_altitude(0);
set_origin_was_armed = false;
}
_analyze->evaluate_all();
}
Coordinates operator()(osmium::Location location) const {
Coordinates c {location.lon(), location.lat()};
if (m_epsg != 4326) {
c = transform(m_crs_wgs84, m_crs_user, Coordinates(deg_to_rad(location.lon()), deg_to_rad(location.lat())));
if (m_crs_user.is_latlong()) {
c.x = rad_to_deg(c.x);
c.y = rad_to_deg(c.y);
}
}
return c;
}
void era_tool_print_state(FILE* stream, era_tool_state_p state) {
fprintf(stream, "%7s: %8.4f m\n",
"x", state->x);
fprintf(stream, "%7s: %8.4f m\n",
"y", state->y);
fprintf(stream, "%7s: %8.4f m\n",
"z", state->z);
fprintf(stream, "%7s: %8.2f deg\n",
"yaw", rad_to_deg(state->yaw));
fprintf(stream, "%7s: %8.2f deg\n",
"roll", rad_to_deg(state->roll));
fprintf(stream, "%7s: %8.2f deg\n",
"opening", rad_to_deg(state->opening));
}
float get_initial_range_guess(float bearing, float heading, uint8_t power){
int8_t bestS = (6-((int8_t)ceilf((3.0*bearing)/M_PI)))%6;
float alpha = pretty_angle(bearing - basis_angle[bestS]); //alpha using infinite approximation
int8_t bestE = (6-((int8_t)ceilf((3.0*(bearing-heading-M_PI))/M_PI)))%6;
float beta = pretty_angle(bearing - heading - basis_angle[bestE] - M_PI); //beta using infinite approximation
//printf("(alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
if((alpha > M_PI_2) || (alpha < -M_PI_2)){
printf("ERROR: alpha out of range (alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
return 0;
}
if((beta > M_PI_2) || (beta < -M_PI_2)){
printf("ERROR: beta out of range (beta: %f, emitter %u)\r\n", beta, bestE);
return 0;
}
//printf("(beta: %f, emitter %u)\r\n", rad_to_deg(beta), bestE);
// expected contribution (using infinite distance approximation)
float amplitude;
float exp_con = sensor_model(alpha)*emitter_model(beta);
if(exp_con > 0) amplitude = brightMeas[bestE][bestS]/exp_con;
else{
printf("ERROR: exp_con (%f) is negative (or zero)!\r\n", exp_con);
return 0;
}
//printf("amp_for_inv: %f\t",amplitude);
float rMagEst = inverse_amplitude_model(amplitude, power);
float RX = rMagEst*cos(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][0]-headingBasis[bestE][0]);
float RY = rMagEst*sin(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][1]-headingBasis[bestE][1]);
float rangeEst = hypotf(RX,RY);
return rangeEst;
}
float waypoint_get_bearing(float num1, float num2, float num3, float num4) {
float dividend;
float divisor;
dividend = atan2((sin(num4-num2)*cos(num3)),(cos(num1)*sin(num3)-(sin(num1)*cos(num3)*cos(num4-num2))));
divisor = 2*PI;
return(rad_to_deg(dividend - divisor * floor (dividend / divisor)));
}
NE_API float angle_between(const transform3f& a, const vector2f& position) {
float result = rad_to_deg(std::atan2(a.position.y - position.y, a.position.x - position.x));
if (result > 180.0f) {
result = 540.0f - result;
} else {
result = 180.0f - result;
}
return result;
}
NE_API float angle_between(const transform3f& a, const transform3f& b) {
const float y = a.position.y - b.position.y + b.scale.y / 2.0f;
const float x = a.position.x - b.position.x + b.scale.y / 2.0f;
float result = rad_to_deg(std::atan2(x, y));
if (result > 180.0f) {
result = 540.0f - result;
} else {
result = 180.0f - result;
}
return result;
}
NE_API float angle_between(const transform3f& a, const vector2f& position, const vector2f& scale) {
const float y = a.position.y - position.y + scale.y / 2.0f;
const float x = a.position.x - position.x + scale.y / 2.0f;
float result = rad_to_deg(std::atan2(y, x));
if (result > 180.0f) {
result = 540.0f - result;
} else {
result = 180.0f - result;
}
return result;
}
//Draw our objects
void render_scene()
{
glClear(GL_COLOR_BUFFER_BIT);
glPushMatrix();
glRotatef(rad_to_deg(x_rot), 1.0f, 0.0f, 0.0f);
glRotatef(rad_to_deg(y_rot), 0.0f, 1.0f, 0.0f);
draw_axes();
glColor3f(1.0f, 0.0f, 0.0f);
//Octagon
glBegin(GL_POLYGON);
glVertex2f(-20.0f, 50.0f);
glVertex2f(20.0f, 50.0f);
glVertex2f(50.0f, 20.0f);
glVertex2f(50.0f, -20.0f);
glVertex2f(20.0f, -50.0f);
glVertex2f(-20.0f, -50.0f);
glVertex2f(-50.0f, -20.0f);
glVertex2f(-50.0f, 20.0f);
glEnd();
glPopMatrix();
}
void era_tool_print_path(FILE* stream, era_tool_path_p path) {
fprintf(stream, "%14s %14s %14s %14s %14s %14s\n",
"x",
"y",
"z",
"yaw",
"roll",
"opening");
int i;
for (i = 0; i < path->num_points; i++) {
fprintf(stream,
"%12.4f m %12.4f m %12.4f m %12.2f deg %12.2f deg %12.2f deg\n",
path->points[i].x,
path->points[i].y,
path->points[i].z,
rad_to_deg(path->points[i].yaw),
rad_to_deg(path->points[i].roll),
rad_to_deg(path->points[i].opening));
}
}
int main(int argc, char **argv) {
config_parser_t parser;
epos_node_t node;
epos_velocity_t velocity;
config_parser_init_default(&parser, &epos_velocity_default_arguments, 0,
"Start EPOS controller in velocity mode",
"Establish the communication with a connected EPOS device and attempt to "
"start the controller in velocity mode. The controller will be stopped "
"if SIGINT is received. The communication interface depends on the "
"momentarily selected alternative of the underlying CANopen library.");
epos_node_init_config_parse(&node, &parser, 0, argc, argv,
config_parser_exit_error);
float target_value = config_get_float(&parser.arguments,
EPOS_VELOCITY_PARAMETER_VELOCITY);
signal(SIGINT, epos_signaled);
epos_node_connect(&node);
error_exit(&node.error);
epos_velocity_init(&velocity, target_value);
epos_velocity_start(&node, &velocity);
error_exit(&node.dev.error);
while (!quit) {
float actual_value = epos_node_get_velocity(&node);
error_exit(&node.error);
fprintf(stdout, "\rAngular velocity: %8.2f deg/s",
rad_to_deg(actual_value));
fflush(stdout);
}
fprintf(stdout, "\n");
epos_velocity_stop(&node);
error_exit(&node.dev.error);
epos_node_disconnect(&node);
error_exit(&node.error);
epos_node_destroy(&node);
config_parser_destroy(&parser);
return 0;
}
void Analyzing_MAVLink_Message_Handler::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) {
_vehicle->set_T(T);
_vehicle->attitude_estimate("ATTITUDE")->set_roll(T, rad_to_deg(msg.roll));
_vehicle->attitude_estimate("ATTITUDE")->set_pitch(T, rad_to_deg(msg.pitch));
_vehicle->attitude_estimate("ATTITUDE")->set_yaw(T, rad_to_deg(msg.yaw));
_vehicle->set_roll(rad_to_deg(msg.roll));
_vehicle->set_pitch(rad_to_deg(msg.pitch));
_vehicle->set_yaw(rad_to_deg(msg.yaw));
_analyze->evaluate_all();
}
int main()
{
double lat_deg,lon_deg,h,var;
int yy,mm,dd;
lat_deg=45.0;
lon_deg=9.0;
h= 1; // altitude in km
mm= 10;
dd= 1;
yy= 11;
var=rad_to_deg(SGMagVar(deg_to_rad(lat_deg),deg_to_rad(lon_deg),h, yymmdd_to_julian_days(yy,mm,dd)));
fprintf(stdout,"var= %4.2f \n",var);
}
float coordinate_calculation::bearing(const FixedPointCoordinate &first_coordinate,
const FixedPointCoordinate &second_coordinate)
{
const float lon_diff =
second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
const float lon_delta = deg_to_rad(lon_diff);
const float lat1 = deg_to_rad(first_coordinate.lat / COORDINATE_PRECISION);
const float lat2 = deg_to_rad(second_coordinate.lat / COORDINATE_PRECISION);
const float y = std::sin(lon_delta) * std::cos(lat2);
const float x =
std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
float result = rad_to_deg(std::atan2(y, x));
while (result < 0.f)
{
result += 360.f;
}
while (result >= 360.f)
{
result -= 360.f;
}
return result;
}
float calc_heading(){
float sum_weight = 0;
float sum_dE_theta_w = 0;
float e_x;
float e_y;
float e_r;
float e_theta; //in degrees
float dE_r;
float dE_theta; //in degrees
float weight;
float dE_theta_w;
for(struct ERROR_VECT* itr = err_list.head->next; itr != err_list.tail; itr = itr->next){
e_x = itr->rct_pt->x + itr->dx;
e_y = itr->rct_pt->y + itr->dy;
e_r = sqrt(square(e_x) + square(e_y));
e_theta = rad_to_deg(atan2(e_x, e_y));
dE_r = itr->rct_pt->magnitude - e_r;
dE_theta = itr->rct_pt->angle - e_theta;
weight = 1/(fabs(dE_r));
dE_theta_w = (dE_theta)*(weight);
sum_weight += weight;
sum_dE_theta_w += dE_theta_w;
}
return sum_dE_theta_w/sum_weight;
}
DEG RobotObserver::deg_getA() const {
return rad_to_deg(_a);
}
void RotatedDC::DrawEllipticArc(const RealPoint& center, const RealSize& size, Radians start, Radians end) {
wxPoint c_ext = tr(center - size/2);
wxSize s_ext = trSizeToBB(size);
dc.DrawEllipticArc(c_ext.x, c_ext.y, s_ext.x, s_ext.y, rad_to_deg(start + angle), rad_to_deg(end + angle));
}
void era_print_state(FILE* stream, era_arm_p arm) {
era_tool_state_t tool_state;
era_joint_state_t joint_state;
era_velocity_state_t vel_state;
era_get_joint_state(arm, &joint_state);
era_kinematics_forward_state(&arm->geometry, &joint_state, &tool_state);
era_get_velocity_state(arm, &vel_state);
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.4f m\n",
"shoulder_yaw", rad_to_deg(joint_state.shoulder_yaw),
rad_to_deg(vel_state.shoulder_yaw), "x", tool_state.x);
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.4f m\n",
"shoulder_roll", rad_to_deg(joint_state.shoulder_roll),
rad_to_deg(vel_state.shoulder_roll), "y", tool_state.y);
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.4f m\n",
"shoulder_pitch", rad_to_deg(joint_state.shoulder_pitch),
rad_to_deg(vel_state.shoulder_pitch), "z", tool_state.z);
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.2f deg\n",
"elbow_pitch", rad_to_deg(joint_state.elbow_pitch),
rad_to_deg(vel_state.elbow_pitch), "yaw", tool_state.yaw);
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.2f deg\n",
"tool_roll", rad_to_deg(joint_state.tool_roll),
rad_to_deg(vel_state.tool_roll), "roll", rad_to_deg(tool_state.roll));
fprintf(stream, "%14s: % 8.2f deg % 8.2f deg/s %7s: % 8.2f deg\n",
"tool_opening", rad_to_deg(joint_state.tool_opening),
rad_to_deg(vel_state.tool_opening), "opening",
rad_to_deg(tool_state.opening));
}
int main(int argc, char **argv) {
config_parser_t parser;
nsick_device_t dev;
transform_pose_t pose;
config_parser_init_default(&parser, &nsick_nod_default_arguments, 0,
"Sweep nodding SICK device",
"Establish the communication with a connected nodding SICK device, "
"attempt to home it, and perform a sweeping profile travel. The "
"sweeping will be stopped if SIGINT is received or the specified "
"number of sweeps is reached.");
nsick_init_config_parse(&dev, &parser, 0, argc, argv,
config_parser_exit_error);
config_parser_destroy(&parser);
int max_sweeps = config_get_int(&parser.arguments,
NSICK_NOD_PARAMETER_NUM_SWEEPS);
signal(SIGINT, nsick_signaled);
nsick_connect(&dev);
error_exit(&dev.error);
fprintf(stderr, "\rHoming: ");
fflush(stderr);
nsick_home(&dev, 0.0);
if (dev.error.code != NSICK_ERROR_WAIT_TIMEOUT) {
fprintf(stderr, "failure\n");
error_exit(&dev.error);
}
while (!quit) {
if (nsick_home_wait(&dev, 0.1) != NSICK_ERROR_WAIT_TIMEOUT) {
fprintf(stderr, "failure\n");
error_exit(&dev.error);
}
}
if (!quit) {
fprintf(stderr, "success\n");
nsick_start(&dev, max_sweeps);
error_exit(&dev.error);
fprintf(stderr, "Origin: %8s %8s %8s %8s %12s\n",
"time [s]", "x [m]", "y [m]", "z [m]", "pitch [deg]");
while (!quit) {
if (nsick_wait(&dev, 0.1) != NSICK_ERROR_WAIT_TIMEOUT)
error_exit(&dev.error);
double time = nsick_get_pose_estimate(&dev, &pose);
fprintf(stdout,
"%16.4f %8.4f %8.4f %8.4f %12.2f\n",
time, pose.x, pose.y, pose.z, rad_to_deg(pose.pitch));
}
nsick_stop(&dev);
error_exit(&dev.error);
}
else {
nsick_home_stop(&dev);
fprintf(stderr, "interrupt\n");
}
nsick_disconnect(&dev);
error_exit(&dev.error);
nsick_destroy(&dev);
return 0;
}
int main(void) {
//various initializations
f3d_uart_init();
f3d_i2c1_init();
delay(10);
f3d_accel_init();
delay(10);
f3d_mag_init();
delay(10);
f3d_gyro_init();
delay(10);
f3d_nunchuk_init();
delay(10);
f3d_user_btn_init();
f3d_lcd_init();
//reset pixels by filling screen RED
f3d_lcd_fillScreen(RED);
//set buffers
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
setvbuf(stderr, NULL, _IONBF, 0);
// initialize gyro constants
START_X = (ST7735_width / 2) - (RECT_WIDTH / 2);
START_Y = (ST7735_height / 2) - (RECT_LENGTH / 2);
GYRO_UPPER_BOUND = 120;
X_MARGIN = (ST7735_width - RECT_WIDTH) / 2;
Y_MARGIN = (ST7735_height - RECT_LENGTH) / 2;
//set centerX and centerY by using global varaibles from library
centerX = ST7735_width / 2;
centerY = ST7735_height / 2;
// constants for the PITCHROLL_MODE
const int barGraphWidth = 40;
const int rollStartY = 30;
const int pitchStartY = 120;
//variables for keeping track of data from previous point
int prevRollX = 0, prevRollY = 0;
int prevPitchX = 0, prevPitchY = 0;
int prevGyroRow = START_X, prevGyroCol = START_Y;
//set float arrays for accel and mag data
float accel_buffer[3];
float mag_buffer[3];
float gyro_buffer[3];
nunchuk_t nunchuk_data;
nunchuk_t *nunchuk_ptr = &nunchuk_data;
//start board in compass mode
int app_mode = COMPASS_MODE;
char *app_mode_title;
while(1) {
//retrieve accel and mag data and insert into their buffers
f3d_accel_read(accel_buffer);
f3d_mag_read(mag_buffer);
f3d_gyro_getdata(gyro_buffer);
f3d_nunchuk_read(nunchuk_ptr);
delay(10);
float Ax1 = accel_buffer[0];
float Ay1 = accel_buffer[1];
float Az1 = accel_buffer[2];
//calcuation of sum of squares
float A_sum_squares = sqrt(Ax1 * Ax1 + Ay1 * Ay1 + Az1 * Az1);
//calculate pitch using accel data and atan2
float pitch = atan2(Ax1, sqrt(Ay1 * Ay1 + Az1 * Az1));
//calculate roll in a similar manner
float roll = atan2(Ay1, sqrt(Ax1 * Ax1 + Az1 * Az1));
//feed mag buffers mag x, y, and z
float Mx = mag_buffer[0];
float My = mag_buffer[1];
float Mz = mag_buffer[2];
//calculate heading in degrees
float Xh = Mx * cos(pitch) + Mz * sin(pitch);
float Yh = Mx * sin(roll) * sin(pitch) + My * cos(roll) - Mz * sin(roll) * cos(pitch);
float headingDegrees = rad_to_deg(atan2(Yh, Xh));
// convert heading degrees to a circular system
float newHeadingDegrees = 0.0;
if (headingDegrees > 90.0) {
// quad I
newHeadingDegrees = fabsf(headingDegrees - 180.0);
} else if (headingDegrees > 0.0) {
// quad II
newHeadingDegrees = 180.0 - headingDegrees;
} else {
// quads III and IV
newHeadingDegrees = 180.0 + fabsf(headingDegrees);
}
int prev_app_mode = app_mode;
// change mode based on nunchuk
const unsigned char joystick_epsilon = 50;
int c_pressed = nunchuk_ptr->z;
int z_pressed = nunchuk_ptr->c;
if (c_pressed != z_pressed) {
// decide based on buttons
if (c_pressed) {
// go right
app_mode = (app_mode + 1) % 4;
} else {
// go left
app_mode = (app_mode + 3) % 4;
}
} else {
// decide based on joystick
const int joystick_x_center = 141;
int joystick_delta = nunchuk_ptr->jx - joystick_x_center;
if (abs(joystick_delta) >= joystick_epsilon) {
// only switch app mode if joystick change is significant
if (joystick_delta < 0) {
// go right
app_mode = (app_mode + 1) % 4;
} else {
// go left
app_mode = (app_mode + 3) % 4;
}
}
}
if (app_mode != prev_app_mode) {
f3d_lcd_fillScreen(RED);
}
//define variables for row and columns of type int
int row, col;
switch(app_mode) {
case COMPASS_MODE: // compass mode
f3d_lcd_fillScreen(RED);
f3d_lcd_drawString(0, 0, "Compass", WHITE, RED);
//draw static white line point upwards on LCD
drawStraightupLine(WHITE);
const float radius = 30.0;
float theta = deg_to_rad(newHeadingDegrees) - (M_PI / 2.0);
//calculat x and y offset
float xOffset = radius * cos(theta);
float yOffset = radius * sin(theta);
//set second point
int x2 = centerX + ((int) xOffset);
int y2 = centerY + ((int) yOffset);
//draw point on the screen at location x2 and y2
f3d_lcd_drawPixel(x2, y2, CYAN);
break;
case PITCHROLL_MODE: // tilt mode
app_mode_title = "Board";
pitchroll_label:
// erase old bars
drawRect(0, rollStartY, prevRollX, prevRollY, RED);
drawRect(0, pitchStartY, prevPitchX, prevPitchY, RED);
//draw the word "Roll" on upper left of LCD
f3d_lcd_drawString(0, 0, "Roll", CYAN, RED);
// title the application
f3d_lcd_drawString((int) ST7735_width * 0.65, 0, app_mode_title, CYAN, RED);
//set color for redrawing of the bars
int rollColor = (roll < 0.0) ? MAGENTA : CYAN;
//calculate perceentage using fabsf (absolute value for float)
float rollPercentage = fabsf(roll) / M_PI;
//calculate rollX and rollY for drawing the roll bar
int rollX = rollPercentage * ST7735_width;
int rollY = rollStartY + barGraphWidth;
drawRect(0, rollStartY, rollX, rollY, rollColor);
//draw the word pitch 90 pixels below Roll
f3d_lcd_drawString(0, 90, "Pitch", CYAN, RED);
//set color for redrawing
int pitchColor = (pitch < 0.0) ? MAGENTA : CYAN;
//calculate pitchPercentage using fabsf(absolute for float)
float pitchPercentage = fabsf(pitch) / M_PI;
//calculate pitchX and pitch Y for drawing Pitch rectangle
int pitchX = pitchPercentage * ST7735_width;
int pitchY = pitchStartY + barGraphWidth;
drawRect(0, pitchStartY, pitchX, pitchY, pitchColor);
//keep track of RollX and PitchX for loop
prevRollX = rollX; prevRollY = rollY;
prevPitchX = pitchX; prevPitchY = pitchY;
break;
case GYRO_MODE: // gyro mode
f3d_lcd_fillScreen(RED);
float gyroDataAvg = (gyro_buffer[0] + gyro_buffer[1] + gyro_buffer[2]) / 3.0f;
float percentageOffset = gyroDataAvg / GYRO_UPPER_BOUND;
int xPixelsFromCenter = (percentageOffset * X_MARGIN) / 1;
int yPixelsFromCenter = (percentageOffset * Y_MARGIN) / 1;
row = START_X + xPixelsFromCenter;
col = START_Y + yPixelsFromCenter;
prevGyroRow = row;
prevGyroCol = col;
drawGyroRect(col, row, WHITE);
break;
case NUNCHUK_MODE:
app_mode_title = "Nunchuk";
const int nunchuk_tilt_upperbound = 1023;
const int nunchuk_tilt_midpoint = nunchuk_tilt_upperbound / 2;
int ax = nunchuk_ptr->ax - nunchuk_tilt_midpoint;
int ay = nunchuk_ptr->ay - nunchuk_tilt_midpoint;
int az = nunchuk_ptr->az;
// calculate pitch and roll of nunchuk
pitch = atan2(ay, sqrt(pow(ay, 2) + pow(az, 2)));
roll = atan2(ax, sqrt(pow(ax, 2) + pow(az, 2)));
// double pitch and roll values to exaggerate their size on bar graph
pitch *= 2;
roll *= 2;
// all the rest is the same as board accelerometer application, so...
goto pitchroll_label;
break;
default:
break;
}
}
}
void laserCallback(const sensor_msgs::LaserScan &laser_scan)
{
if (ping_index_ < 0)
{
//for first message received, set up the desired index of LIDAR range to eval
angle_min_ = laser_scan.angle_min;
angle_max_ = laser_scan.angle_max;
angle_increment_ = laser_scan.angle_increment;
range_min_ = laser_scan.range_min;
range_max_ = laser_scan.range_max;
// what is the index of the ping that is straight ahead?
// BETTER would be to use transforms, which would reference how the LIDAR is mounted;
// but this will do for simple illustration
ping_index_ = (int)((angle_max_ - angle_min_) / angle_increment_) / 2;
points_per_degree = ((ping_index_ * 2) / rad_to_deg(angle_max_ - angle_min_));
int consider_pings_one_side = points_per_degree * (FIELD_OF_VIEW / 2);
ping_low_limit_index_ = ping_index_ - consider_pings_one_side;
ping_high_limit_index_ = ping_index_ + consider_pings_one_side;
// Print out some debug info
ROS_INFO("LIDAR setup: ping_index = %d", ping_index_);
ROS_INFO("LIDAR setup: angle_min = %lfdeg", rad_to_deg(angle_min_));
ROS_INFO("LIDAR setup: angle_max = %lfdeg", rad_to_deg(angle_max_));
ROS_INFO("LIDAR setup: angle_inc = %lfdeg", rad_to_deg(angle_increment_));
ROS_INFO("LIDAR setup: points per degree = %lf", points_per_degree);
ROS_INFO("LIDAR setup: checking indicies %d through %d", ping_low_limit_index_, ping_high_limit_index_);
}
ping_dist_in_front_ = laser_scan.ranges[ping_index_];
int too_close_count = 0;
for (int i = ping_low_limit_index_; i < ping_high_limit_index_; i++)
{
double ping_dist_in_front_ = laser_scan.ranges[i];
double cos_distance = ping_dist_in_front_ * std::cos(index_to_rad_off_x(i));
if (!std::isinf(ping_dist_in_front_))
{
//ROS_INFO("ping dist %lf at angle %lfdeg", ping_dist_in_front_, index_to_deg_off_x(i));
}
if (cos_distance < MIN_SAFE_DISTANCE)
{
ROS_WARN("There appears to be something in front of us, %lfdeg off center, %lfm away in x", index_to_deg_off_x(i), cos_distance);
if (++too_close_count > ALLOWABLE_CLOSE_PINGS)
{
ROS_WARN("DANGER, WILL ROBINSON!!");
laser_alarm_ = true;
break;
}
}
else
{
laser_alarm_ = false;
}
}
ping_dist_in_front_ = laser_scan.ranges[ping_index_];
std_msgs::Bool lidar_alarm_msg;
lidar_alarm_msg.data = laser_alarm_;
lidar_alarm_publisher_.publish(lidar_alarm_msg);
std_msgs::Float32 lidar_dist_msg;
lidar_dist_msg.data = ping_dist_in_front_;
lidar_dist_publisher_.publish(lidar_dist_msg);
}
inline double y_to_lat(double y) { // not constexpr because math functions aren't
return rad_to_deg(2 * std::atan(std::exp(y / earth_radius_for_epsg3857)) - osmium::geom::PI/2);
}
constexpr inline double x_to_lon(double x) {
return rad_to_deg(x) / earth_radius_for_epsg3857;
}
