diff --git a/SerialInterface.cpp b/SerialInterface.cpp index 1ac8225951..a7be21761a 100644 --- a/SerialInterface.cpp +++ b/SerialInterface.cpp @@ -56,14 +56,14 @@ int init_port(int baud) return 0; // Success! } -int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels) +int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels, int * samples_averaged, int * LED_state) { // Channels: // 0, 1 = Head Pitch and Yaw // 2,3,4 = Head X,Y,Z Acceleration // int samples_read = 0; - const float AVG_RATE[] = {0.001, 0.001, 0.001, 0.001, 0.001}; + const float AVG_RATE[] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001}; char bufchar[1]; while (read(serial_fd, bufchar, 1) > 0) { @@ -76,12 +76,15 @@ int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_chan // At end - Extract value from string to variables if (serial_buffer[0] != 'p') { - sscanf(serial_buffer, "%d %d %d %d %d", /* Needs to match Num Channels */ + sscanf(serial_buffer, "%d %d %d %d %d %d %d %d", /* Needs to match Num Channels */ &adc_channels[0], &adc_channels[1], &adc_channels[2], &adc_channels[3], - &adc_channels[4] + &adc_channels[4], + &adc_channels[5], + samples_averaged, + LED_state ); for (int i = 0; i < NUM_CHANNELS; i++) { diff --git a/SerialInterface.h b/SerialInterface.h index cc2d76600e..fc1abf180d 100644 --- a/SerialInterface.h +++ b/SerialInterface.h @@ -7,18 +7,19 @@ #define interface_SerialInterface_h int init_port (int baud); -int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels); +int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels, int * samples_averaged, int * LED_state); -#define NUM_CHANNELS 5 +#define NUM_CHANNELS 6 #define SERIAL_PORT_NAME "/dev/tty.usbmodem641" // Acceleration sensors, in screen/world coord system (X = left/right, Y = Up/Down, Z = fwd/back) -#define ACCEL_X 3 -#define ACCEL_Y 4 -#define ACCEL_Z 2 +#define ACCEL_X 4 +#define ACCEL_Y 5 +#define ACCEL_Z 3 // Gyro sensors, in coodinate system of head/airplane #define PITCH_RATE 0 #define YAW_RATE 1 +#define ROLL_RATE 2 #endif diff --git a/cloud.cpp b/cloud.cpp index 329b840ab5..155540bc01 100644 --- a/cloud.cpp +++ b/cloud.cpp @@ -30,9 +30,9 @@ Cloud::Cloud(int num, particles[i].position.y = y; particles[i].position.z = z; - particles[i].velocity.x = 0; //randFloat() - 0.5; - particles[i].velocity.y = 0; //randFloat() - 0.5; - particles[i].velocity.z = 0; //randFloat() - 0.5; + particles[i].velocity.x = randFloat() - 0.5; + particles[i].velocity.y = randFloat() - 0.5; + particles[i].velocity.z = randFloat() - 0.5; float color_mult = 1 - COLOR_MIN; particles[i].color = glm::vec3(x*color_mult/WORLD_SIZE + COLOR_MIN, @@ -78,16 +78,16 @@ void Cloud::simulate (float deltaTime) { for (i = 0; i < count; ++i) { // Update position - //particles[i].position += particles[i].velocity*deltaTime; - particles[i].position += particles[i].velocity; + particles[i].position += particles[i].velocity*deltaTime; + //particles[i].position += particles[i].velocity; // Decay Velocity (Drag) - const float CONSTANT_DAMPING = 1.0; + const float CONSTANT_DAMPING = 0.5; particles[i].velocity *= (1.f - CONSTANT_DAMPING*deltaTime); // Interact with Field - const float FIELD_COUPLE = 0.0000001; - field_interact(&particles[i].position, &particles[i].velocity, &particles[i].color, FIELD_COUPLE); + const float FIELD_COUPLE = 0.005; //0.0000001; + field_interact(deltaTime, &particles[i].position, &particles[i].velocity, &particles[i].color, FIELD_COUPLE); // Bounce or Wrap if (wrapBounds) { diff --git a/field.cpp b/field.cpp index 011fb6515f..f450c38525 100644 --- a/field.cpp +++ b/field.cpp @@ -62,16 +62,16 @@ void field_add(float* add, float *pos) } } -void field_interact(glm::vec3 * pos, glm::vec3 * vel, glm::vec3 * color, float coupling) { +void field_interact(float dt, glm::vec3 * pos, glm::vec3 * vel, glm::vec3 * color, float coupling) { int index = (int)(pos->x/WORLD_SIZE*10.0) + (int)(pos->y/WORLD_SIZE*10.0)*10 + (int)(pos->z/WORLD_SIZE*10.0)*100; if ((index >= 0) && (index < FIELD_ELEMENTS)) { // Add velocity to particle from field - *vel += field[index].val; + *vel += field[index].val*dt; // Add back to field from particle velocity - glm::vec3 temp = *vel; + glm::vec3 temp = *vel*dt; temp *= coupling; field[index].val += temp; @@ -127,7 +127,8 @@ void field_simulate(float dt) { field[i].val += add; } else { - field[i].val *= 0.999; + const float CONSTANT_DAMPING = 0.5; + field[i].val *= (1.f - CONSTANT_DAMPING*dt); //field[i].val.x += (randFloat() - 0.5)*0.01*FIELD_SCALE; //field[i].val.y += (randFloat() - 0.5)*0.01*FIELD_SCALE; //field[i].val.z += (randFloat() - 0.5)*0.01*FIELD_SCALE; @@ -141,7 +142,7 @@ void field_render() { int i; float fx, fy, fz; - float scale_view = 1000.0; + float scale_view = 0.1; glDisable(GL_LIGHTING); glColor3f(0, 1, 0); diff --git a/field.h b/field.h index 33004554d2..354a4a8149 100644 --- a/field.h +++ b/field.h @@ -35,7 +35,7 @@ void field_init(); int field_value(float *ret, float *pos); void field_render(); void field_add(float* add, float *loc); -void field_interact(glm::vec3 * pos, glm::vec3 * vel, glm::vec3 * color, float coupling); +void field_interact(float dt, glm::vec3 * pos, glm::vec3 * vel, glm::vec3 * color, float coupling); void field_simulate(float dt); glm::vec3 hsv2rgb(glm::vec3 in); #endif diff --git a/head.cpp b/head.cpp index d2b2a0ed02..46dd25028b 100644 --- a/head.cpp +++ b/head.cpp @@ -29,7 +29,7 @@ const float DECAY = 0.1; Head::Head() { PupilSize = 0.10; - interPupilDistance = 0.5; + interPupilDistance = 0.6; interBrowDistance = 0.75; NominalPupilSize = 0.10; EyebrowPitch[0] = EyebrowPitch[1] = BrowPitchAngle[0]; @@ -40,10 +40,11 @@ Head::Head() MouthWidth = 1.0; MouthHeight = 0.2; EyeballPitch[0] = EyeballPitch[1] = 0; + EyeballScaleX = 1.2; EyeballScaleY = 1.5; EyeballScaleZ = 1.0; EyeballYaw[0] = EyeballYaw[1] = 0; PitchTarget = YawTarget = 0; NoiseEnvelope = 1.0; - PupilConverge = 2.1; + PupilConverge = 5.0; leanForward = 0.0; leanSideways = 0.0; setNoise(0); @@ -52,32 +53,36 @@ Head::Head() void Head::reset() { position = glm::vec3(0,0,0); - Pitch = 0; - Yaw = 0; + Pitch = Yaw = Roll = 0; leanForward = leanSideways = 0; } void Head::UpdatePos(float frametime, int * adc_channels, float * avg_adc_channels, int head_mirror, glm::vec3 * gravity) // Using serial data, update avatar/render position and angles { - float measured_pitch_rate = adc_channels[0] - avg_adc_channels[0]; - float measured_yaw_rate = adc_channels[1] - avg_adc_channels[1]; - float measured_lateral_accel = adc_channels[3] - avg_adc_channels[3]; - float measured_fwd_accel = avg_adc_channels[2] - adc_channels[2]; + float measured_pitch_rate = adc_channels[PITCH_RATE] - avg_adc_channels[PITCH_RATE]; + float measured_yaw_rate = adc_channels[YAW_RATE] - avg_adc_channels[YAW_RATE]; + float measured_lateral_accel = adc_channels[ACCEL_X] - avg_adc_channels[ACCEL_X]; + float measured_fwd_accel = avg_adc_channels[ACCEL_Z] - adc_channels[ACCEL_Z]; + float measured_roll_rate = adc_channels[ROLL_RATE] - avg_adc_channels[ROLL_RATE]; // Update avatar head position based on measured gyro rates const float HEAD_ROTATION_SCALE = 0.20; + const float HEAD_ROLL_SCALE = 0.50; const float HEAD_LEAN_SCALE = 0.02; if (head_mirror) { addYaw(measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); + addRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime); addLean(measured_lateral_accel * frametime * HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } else { addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); + addRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime); addLean(measured_lateral_accel * frametime * -HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } + // Try to measure absolute roll from sensors const float MIN_ROLL = 3.0; glm::vec3 v1(gravity->x, gravity->y, 0); @@ -112,7 +117,7 @@ void Head::simulate(float deltaTime) // Move toward new target Pitch += (PitchTarget - Pitch)*22*deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ; Yaw += (YawTarget - Yaw)*22*deltaTime; // (1.f - DECAY*deltaTime); - //Roll *= (1.f - DECAY*deltaTime); + Roll *= (1.f - DECAY*deltaTime); } leanForward *= (1.f - DECAY*30.f*deltaTime); @@ -166,16 +171,18 @@ void Head::render() glLoadIdentity(); glTranslatef(0.f, 0.f, -7.f); glTranslatef(leanSideways, 0.f, leanForward); + glRotatef(Yaw/2.0, 0, 1, 0); glRotatef(Pitch/2.0, 1, 0, 0); glRotatef(Roll/2.0, 0, 0, 1); + // Overall scale of head - glScalef(2.0, 2.0, 2.0); + glScalef(1.5, 2.0, 2.0); glColor3fv(skinColor); // Head - glutSolidSphere(1, 15, 15); + glutSolidSphere(1, 30, 30); // Ears glPushMatrix(); @@ -184,7 +191,7 @@ void Head::render() { glPushMatrix(); glScalef(0.5, 0.75, 1.0); - glutSolidSphere(0.5, 15, 15); + glutSolidSphere(0.5, 30, 30); glPopMatrix(); glTranslatef(-2, 0, 0); } @@ -221,29 +228,45 @@ void Head::render() glTranslatef(0, 1.0, 0); + + glTranslatef(-interPupilDistance/2.0,-0.68,0.7); // Right Eye - glTranslatef(-0.25,-0.5,0.7); + glRotatef(-10, 1, 0, 0); glColor3fv(eyeColor); - glutSolidSphere(0.25, 15, 15); + glPushMatrix(); + { + glTranslatef(interPupilDistance/10.0, 0, 0.05); + glRotatef(20, 0, 0, 1); + glScalef(EyeballScaleX, EyeballScaleY, EyeballScaleZ); + glutSolidSphere(0.25, 30, 30); + } + glPopMatrix(); // Right Pupil glPushMatrix(); glRotatef(EyeballPitch[1], 1, 0, 0); glRotatef(EyeballYaw[1] + PupilConverge, 0, 1, 0); glTranslatef(0,0,.25); glColor3f(0,0,0); - glutSolidSphere(PupilSize, 10, 10); + glutSolidSphere(PupilSize, 15, 15); glPopMatrix(); // Left Eye glColor3fv(eyeColor); glTranslatef(interPupilDistance, 0, 0); - glutSolidSphere(0.25f, 15, 15); + glPushMatrix(); + { + glTranslatef(-interPupilDistance/10.0, 0, .05); + glRotatef(-20, 0, 0, 1); + glScalef(EyeballScaleX, EyeballScaleY, EyeballScaleZ); + glutSolidSphere(0.25, 30, 30); + } + glPopMatrix(); // Left Pupil glPushMatrix(); glRotatef(EyeballPitch[0], 1, 0, 0); glRotatef(EyeballYaw[0] - PupilConverge, 0, 1, 0); glTranslatef(0,0,.25); glColor3f(0,0,0); - glutSolidSphere(PupilSize, 10, 10); + glutSolidSphere(PupilSize, 15, 15); glPopMatrix(); diff --git a/head.h b/head.h index 5875114e77..b0182cd498 100644 --- a/head.h +++ b/head.h @@ -27,6 +27,7 @@ class Head { float EyeballYaw[2]; float EyebrowPitch[2]; float EyebrowRoll[2]; + float EyeballScaleX, EyeballScaleY, EyeballScaleZ; float interPupilDistance; float interBrowDistance; float NominalPupilSize; diff --git a/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate b/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate index e02456d717..5069cead64 100644 Binary files a/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate and b/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate differ diff --git a/main.cpp b/main.cpp index c03f08025e..f366efad3a 100644 --- a/main.cpp +++ b/main.cpp @@ -51,8 +51,6 @@ using namespace std; -// Junk for talking to the Serial Port -int serial_on = 0; // Is serial connection on/off? System will try int audio_on = 0; // Whether to turn on the audio support int simulate_on = 1; @@ -93,7 +91,7 @@ ParticleSystem balls(0, ); -Cloud cloud(200000, // Particles +Cloud cloud(100000, // Particles box, // Bounding Box false // Wrap ); @@ -103,7 +101,7 @@ float cubes_scale[MAX_CUBES]; float cubes_color[MAX_CUBES*3]; int cube_count = 0; -#define RENDER_FRAME_MSECS 10 +#define RENDER_FRAME_MSECS 5 #define SLEEP 0 float yaw =0.f; // The yaw, pitch for the avatar head @@ -146,19 +144,15 @@ int mouse_pressed = 0; // true if mouse has been pressed (clear when finishe int speed; // -// Serial I/O channel mapping: -// -// 0 Head Gyro Pitch -// 1 Head Gyro Yaw -// 2 Head Accelerometer X -// 3 Head Accelerometer Z -// 4 Hand Accelerometer X -// 5 Hand Accelerometer Y -// 6 Hand Accelerometer Z +// Serial USB Variables // +int serial_on = 0; +int latency_display = 1; int adc_channels[NUM_CHANNELS]; float avg_adc_channels[NUM_CHANNELS]; +int sensor_samples = 0; +int sensor_LED = 0; glm::vec3 gravity; int first_measurement = 1; int samplecount = 0; @@ -214,9 +208,14 @@ void display_stats(void) drawtext(10, 15, 0.10, 0, 1.0, 0, legend); char stats[200]; - sprintf(stats, "FPS = %3.0f, Ping = %4.1f Packets/Sec = %d, Bytes/sec = %d", + sprintf(stats, "FPS = %3.0f, Ping = %4.1f Pkts/s = %d, Bytes/s = %d", FPS, ping_msecs, packets_per_second, bytes_per_second); drawtext(10, 30, 0.10, 0, 1.0, 0, stats); + if (serial_on) { + sprintf(stats, "ADC samples = %d, LED = %d", + sensor_samples, sensor_LED); + drawtext(500, 30, 0.10, 0, 1.0, 0, stats); + } /* char adc[200]; @@ -296,7 +295,7 @@ void init(void) cube_count = index; // Recursive build - /* + float location[] = {0,0,0}; float scale = 10.0; int j = 0; @@ -316,11 +315,11 @@ void init(void) // Call readsensors for a while to get stable initial values on sensors printf( "Stabilizing sensors... " ); gettimeofday(&timer_start, NULL); - read_sensors(1, &avg_adc_channels[0], &adc_channels[0]); + read_sensors(1, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); int done = 0; while (!done) { - read_sensors(0, &avg_adc_channels[0], &adc_channels[0]); + read_sensors(0, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); gettimeofday(&timer_end, NULL); if (diffclock(timer_start,timer_end) > 1000) done = 1; } @@ -367,7 +366,7 @@ void reset_sensors() myHead.reset(); myHand.reset(); - if (serial_on) read_sensors(1, &avg_adc_channels[0], &adc_channels[0]); + if (serial_on) read_sensors(1, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); } void update_pos(float frametime) @@ -493,8 +492,6 @@ void update_pos(float frametime) void display(void) { - - glEnable (GL_DEPTH_TEST); glEnable(GL_LIGHTING); glEnable(GL_LINE_SMOOTH); @@ -627,6 +624,17 @@ void display(void) disp_x += GAP; } + // Display Serial latency block + if (latency_display && sensor_LED) { + glColor3f(1,0,0); + glBegin(GL_QUADS); { + glVertex2f(WIDTH - 100, HEIGHT - 100); + glVertex2f(WIDTH, HEIGHT - 100); + glVertex2f(WIDTH, HEIGHT); + glVertex2f(WIDTH - 100, HEIGHT); + } + glEnd(); + } } if (stats_on) display_stats(); @@ -774,7 +782,8 @@ void idle(void) // Read network packets read_network(); // Read serial data - if (serial_on) samplecount += read_sensors(0, &avg_adc_channels[0], &adc_channels[0]); + if (serial_on) samplecount += read_sensors(0, &avg_adc_channels[0], &adc_channels[0], + &sensor_samples, &sensor_LED); if (SLEEP) {