void SimulatorSingleton::add_position(world_vector p)
{
model.position.x += p.x;
model.position.y += p.y;
model.position.z += p.z;
set_target_depth(POOL_HEIGHT - model.position.y);
}
void SimulatorSingleton::zero_speed()
{
model.speed = 0;
model.depth_speed = 0;
model.angular_speed = 0;
model.accel = 0;
model.angular_accel = 0;
model.depth_accel = 0;
set_target_yaw(model.angle.yaw);
set_target_depth(POOL_HEIGHT - model.position.y);
}
void pid_init () // call this anytime before calling calculate_pid
{
// set up PID values
set_pid_constants_pitch(PITCH_CONST_P, PITCH_CONST_I, PITCH_CONST_D, PITCH_ALPHA);
set_pid_constants_roll(ROLL_CONST_P, ROLL_CONST_I, ROLL_CONST_D, ROLL_ALPHA);
set_pid_constants_yaw(YAW_CONST_P, YAW_CONST_I, YAW_CONST_D, YAW_ALPHA);
set_pid_constants_depth(DEPTH_CONST_P, DEPTH_CONST_I, DEPTH_CONST_D, DEPTH_ALPHA);
// Initialize target orientation
set_target_depth(0);
set_target_speed(0);
set_target_heading(0);
// Initialize motor linearization lookup table
init_lookup();
}
void SimulatorSingleton::run_sim()
{
int argc = 0;
char *argv[1];
unsigned width, height;
read_common_mv_setting ("IMG_WIDTH_COMMON", width);
read_common_mv_setting ("IMG_HEIGHT_COMMON", height);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_SINGLE | GLUT_DEPTH | GLUT_RGB | GLUT_DOUBLE);
glutInitWindowSize (width, height);
glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION);
glutInitWindowPosition(10, 0);
dwn_window = glutCreateWindow ("Down Cam");
glutPositionWindow(0, 400);
glutReshapeFunc (::sim_reshape);
glutDisplayFunc (::sim_display);
glutIdleFunc (::sim_idle);
glutKeyboardFunc (::sim_keyboard);
glutCloseFunc (::sim_close_window);
init_sim();
fwd_window = glutCreateWindow ("Forwards Cam");
glutReshapeFunc (::sim_reshape);
glutDisplayFunc (::sim_display);
glutIdleFunc (::sim_idle);
glutKeyboardFunc (::sim_keyboard);
glutCloseFunc (::sim_close_window);
init_sim();
img_fwd = cvCreateImage (cvSize(width,height), IPL_DEPTH_8U, 3);
img_dwn = cvCreateImage (cvSize(width,height), IPL_DEPTH_8U, 3);
// set initial position
std::string start_at;
read_mv_setting(SIM_SETTINGS_FILE, "START_AT", start_at);
float start_x = 0.f;
float start_y = 0.f;
float start_z = 0.f;
float start_yaw = 0.f;
if (start_at == "GATE") {
start_x = -3.f;
start_y = 7.5f;
start_z = 16.f;
} else if (start_at == "BUOY") {
start_x = -3.3f;
start_y = 4.5f;
start_z = 3.f;
start_yaw = 18.f;
} else if (start_at == "GOALPOST") {
start_x = 1.f;
start_y = 2.5f;
start_z = -4.2f;
start_yaw = 80.f;
} else if (start_at == "MULTIPATH") {
start_x = 7.f;
start_y = 2.5f;
start_z = -5.f;
start_yaw = 80.f;
} else if (start_at == "TORPEDO_TARGET") {
start_x = 10.1f;
start_y = 2.35f;
start_z = -1.9f;
start_yaw = -170.f;
} else if (start_at == "MARKER_DROPPER") {
start_x = 10.8f;
start_y = 2.5f;
start_z = -7.1f;
start_yaw = -4.f;
}
model.position.x = start_x;
model.position.y = start_y;
model.position.z = start_z;
model.angle.yaw = start_yaw;
set_target_depth(POOL_HEIGHT - start_y);
set_target_yaw(start_yaw);
// Sim has initialized
sem_post(&sema);
glutMainLoop();
}
