diff --git a/SerialInterface.cpp b/SerialInterface.cpp index e3b63fcdae..1ac8225951 100644 --- a/SerialInterface.cpp +++ b/SerialInterface.cpp @@ -60,10 +60,10 @@ int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_chan { // Channels: // 0, 1 = Head Pitch and Yaw - // 2,3,4 = Head XYZ Acceleration + // 2,3,4 = Head X,Y,Z Acceleration // int samples_read = 0; - const float AVG_RATE[] = {0.001, 0.001, 0.01, 0.01, 0.01}; + const float AVG_RATE[] = {0.001, 0.001, 0.001, 0.001, 0.001}; char bufchar[1]; while (read(serial_fd, bufchar, 1) > 0) { diff --git a/SerialInterface.h b/SerialInterface.h index 3fd7a2146e..cc2d76600e 100644 --- a/SerialInterface.h +++ b/SerialInterface.h @@ -12,4 +12,13 @@ int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_chan #define NUM_CHANNELS 5 #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 + +// Gyro sensors, in coodinate system of head/airplane +#define PITCH_RATE 0 +#define YAW_RATE 1 + #endif diff --git a/head.cpp b/head.cpp index 98cbf0d0d3..d2b2a0ed02 100644 --- a/head.cpp +++ b/head.cpp @@ -9,6 +9,7 @@ #include #include "head.h" #include "util.h" +#include "glm/gtx/vector_angle.hpp" float skinColor[] = {1.0, 0.84, 0.66}; float browColor[] = {210.0/255.0, 105.0/255.0, 30.0/255.0}; @@ -56,14 +57,7 @@ void Head::reset() leanForward = leanSideways = 0; } -// Read the sensors -void readSensors() -{ - -} - -/* -void update_pos(float frametime) +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]; @@ -75,111 +69,28 @@ void update_pos(float frametime) const float HEAD_ROTATION_SCALE = 0.20; const float HEAD_LEAN_SCALE = 0.02; if (head_mirror) { - myHead.addYaw(measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); - myHead.addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addLean(measured_lateral_accel * frametime * HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); + addYaw(measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); + addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); + addLean(measured_lateral_accel * frametime * HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } else { - myHead.addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addLean(measured_lateral_accel * frametime * -HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); + addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); + addPitch(measured_pitch_rate * -HEAD_ROTATION_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); + glm::vec3 v2(adc_channels[ACCEL_X], adc_channels[ACCEL_Y], 0); + float newRoll = acos(glm::dot(glm::normalize(v1), glm::normalize(v2))) ; + if (newRoll != NAN) { + newRoll *= 1000.0; + if (newRoll > MIN_ROLL) { + if (adc_channels[ACCEL_X] > gravity->x) newRoll *= -1.0; + //SetRoll(newRoll); + } } - // Decay avatar head back toward zero - //pitch *= (1.f - 5.0*frametime); - //yaw *= (1.f - 7.0*frametime); - - // Update head_mouse model - const float MIN_MOUSE_RATE = 30.0; - const float MOUSE_SENSITIVITY = 0.1; - if (powf(measured_yaw_rate*measured_yaw_rate + - measured_pitch_rate*measured_pitch_rate, 0.5) > MIN_MOUSE_RATE) - { - head_mouse_x -= measured_yaw_rate*MOUSE_SENSITIVITY; - head_mouse_y += measured_pitch_rate*MOUSE_SENSITIVITY*(float)HEIGHT/(float)WIDTH; - } - head_mouse_x = max(head_mouse_x, 0); - head_mouse_x = min(head_mouse_x, WIDTH); - head_mouse_y = max(head_mouse_y, 0); - head_mouse_y = min(head_mouse_y, HEIGHT); - - // Update render direction (pitch/yaw) based on measured gyro rates - const int MIN_YAW_RATE = 300; - const float YAW_SENSITIVITY = 0.03; - const int MIN_PITCH_RATE = 300; - const float PITCH_SENSITIVITY = 0.04; - - if (fabs(measured_yaw_rate) > MIN_YAW_RATE) - { - if (measured_yaw_rate > 0) - render_yaw_rate -= (measured_yaw_rate - MIN_YAW_RATE) * YAW_SENSITIVITY * frametime; - else - render_yaw_rate -= (measured_yaw_rate + MIN_YAW_RATE) * YAW_SENSITIVITY * frametime; - } - if (fabs(measured_pitch_rate) > MIN_PITCH_RATE) - { - if (measured_pitch_rate > 0) - render_pitch_rate += (measured_pitch_rate - MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime; - else - render_pitch_rate += (measured_pitch_rate + MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime; - } - render_yaw += render_yaw_rate; - render_pitch += render_pitch_rate; - - // Decay render_pitch toward zero because we never look constantly up/down - render_pitch *= (1.f - 2.0*frametime); - - // Decay angular rates toward zero - render_pitch_rate *= (1.f - 5.0*frametime); - render_yaw_rate *= (1.f - 7.0*frametime); - - // Update slide left/right based on accelerometer reading - const int MIN_LATERAL_ACCEL = 20; - const float LATERAL_SENSITIVITY = 0.001; - if (fabs(measured_lateral_accel) > MIN_LATERAL_ACCEL) - { - if (measured_lateral_accel > 0) - lateral_vel += (measured_lateral_accel - MIN_LATERAL_ACCEL) * LATERAL_SENSITIVITY * frametime; - else - lateral_vel += (measured_lateral_accel + MIN_LATERAL_ACCEL) * LATERAL_SENSITIVITY * frametime; - } - - //slide += lateral_vel; - lateral_vel *= (1.f - 4.0*frametime); - - // Update fwd/back based on accelerometer reading - const int MIN_FWD_ACCEL = 20; - const float FWD_SENSITIVITY = 0.001; - - if (fabs(measured_fwd_accel) > MIN_FWD_ACCEL) - { - if (measured_fwd_accel > 0) - fwd_vel += (measured_fwd_accel - MIN_FWD_ACCEL) * FWD_SENSITIVITY * frametime; - else - fwd_vel += (measured_fwd_accel + MIN_FWD_ACCEL) * FWD_SENSITIVITY * frametime; - - } - // Decrease forward velocity - fwd_vel *= (1.f - 4.0*frametime); - - // Update forward vector based on pitch and yaw - fwd_vec[0] = -sinf(render_yaw*PI/180); - fwd_vec[1] = sinf(render_pitch*PI/180); - fwd_vec[2] = cosf(render_yaw*PI/180); - - // Advance location forward - location[0] += fwd_vec[0]*fwd_vel; - location[1] += fwd_vec[1]*fwd_vel; - location[2] += fwd_vec[2]*fwd_vel; - - // Slide location sideways - location[0] += fwd_vec[2]*-lateral_vel; - location[2] += fwd_vec[0]*lateral_vel; - - // Update head and manipulator objects with object with current location - myHead.setPos(glm::vec3(location[0], location[1], location[2])); - balls.updateHand(myHead.getPos() + myHand.getPos(), glm::vec3(0,0,0), myHand.getRadius()); } -*/ - void Head::addLean(float x, float z) { // Add Body lean as impulse @@ -201,7 +112,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); diff --git a/head.h b/head.h index be228137ec..5875114e77 100644 --- a/head.h +++ b/head.h @@ -13,6 +13,7 @@ #include "field.h" #include "world.h" #include +#include "SerialInterface.h" class Head { float noise; @@ -51,11 +52,15 @@ class Head { public: Head(void); void reset(); + void UpdatePos(float frametime, int * adc_channels, float * avg_adc_channels, + int head_mirror, glm::vec3 * gravity); void setNoise (float mag) { noise = mag; } void setPitch(float p) {Pitch = p; } void setYaw(float y) {Yaw = y; } + void SetRoll(float r) {Roll = r; }; void addPitch(float p) {Pitch -= p; } void addYaw(float y){Yaw -= y; } + void addRoll(float r){Roll += r; } void addLean(float x, float z); void getPitch(float); void render(); diff --git a/interface.xcodeproj/project.pbxproj b/interface.xcodeproj/project.pbxproj index 8930389ea0..613476cb65 100644 --- a/interface.xcodeproj/project.pbxproj +++ b/interface.xcodeproj/project.pbxproj @@ -140,11 +140,11 @@ isa = PBXGroup; children = ( 08FB7796FE84155DC02AAC07 /* main.cpp */, + D4EE3BC015E746E900EE4C89 /* world.h */, D409B9A6165CA7A50099B0B3 /* agent.h */, D409B9A7165CA7BB0099B0B3 /* agent.cpp */, D409B988165849030099B0B3 /* cloud.h */, D409B989165849180099B0B3 /* cloud.cpp */, - D4EE3BC015E746E900EE4C89 /* world.h */, B6BDAE4315F6BE53002A07DF /* particle.cpp */, B6BDAE4115F6BE4D002A07DF /* particle.h */, D4EE3BBD15E7465700EE4C89 /* field.cpp */, diff --git a/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate b/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate index 1aded4aa69..9c6cf6345d 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 3596d20183..bed0910463 100644 --- a/main.cpp +++ b/main.cpp @@ -76,7 +76,7 @@ int bytescount = 0; int target_x, target_y; int target_display = 0; -int head_mirror = 0; // Whether to mirror the head when viewing it +int head_mirror = 1; // Whether to mirror the head when viewing it int WIDTH = 1200; int HEIGHT = 800; @@ -95,7 +95,7 @@ ParticleSystem balls(0, 0.0 // Gravity ); -Cloud cloud(0, // Particles +Cloud cloud(100000, // Particles box, // Bounding Box false // Wrap ); @@ -161,6 +161,7 @@ int speed; int adc_channels[NUM_CHANNELS]; float avg_adc_channels[NUM_CHANNELS]; +glm::vec3 gravity; int first_measurement = 1; int samplecount = 0; @@ -277,16 +278,32 @@ void init(void) } int index = 0; + while (index < MAX_CUBES) { + cubes_position[index*3] = randFloat()*WORLD_SIZE; + cubes_position[index*3+1] = randFloat()*WORLD_SIZE; + cubes_position[index*3+2] = randFloat()*WORLD_SIZE; + cubes_scale[index] = WORLD_SIZE/powf(2,2+rand()%8); + float color = randFloat(); + cubes_color[index*3] = color; + cubes_color[index*3 + 1] = color; + cubes_color[index*3 + 2] = color; + index++; + } + cube_count = index; + + // Recursive build + /* float location[] = {0,0,0}; float scale = 10.0; int j = 0; - while (index < (MAX_CUBES/2)) { + + while (index < 4) { //(index < (MAX_CUBES/2)) { index = 0; j++; makeCubes(location, scale, &index, cubes_position, cubes_scale, cubes_color); std::cout << "Run " << j << " Made " << index << " cubes\n"; cube_count = index; - } + }*/ //load_png_as_texture(texture_filename); @@ -303,6 +320,11 @@ void init(void) gettimeofday(&timer_end, NULL); if (diffclock(timer_start,timer_end) > 1000) done = 1; } + gravity.x = avg_adc_channels[ACCEL_X]; + gravity.y = avg_adc_channels[ACCEL_Y]; + gravity.z = avg_adc_channels[ACCEL_Z]; + + std::cout << "Gravity: " << gravity.x << "," << gravity.y << "," << gravity.z << "\n"; printf( "Done.\n" ); } @@ -347,27 +369,13 @@ void reset_sensors() void update_pos(float frametime) // 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]; + + myHead.UpdatePos(frametime, &adc_channels[0], &avg_adc_channels[0], head_mirror, &gravity); - // Update avatar head position based on measured gyro rates - const float HEAD_ROTATION_SCALE = 0.20; - const float HEAD_LEAN_SCALE = 0.02; - if (head_mirror) { - myHead.addYaw(measured_yaw_rate * HEAD_ROTATION_SCALE * frametime); - myHead.addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addLean(measured_lateral_accel * frametime * HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); - } else { - myHead.addYaw(measured_yaw_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime); - myHead.addLean(measured_lateral_accel * frametime * -HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); - } - // Decay avatar head back toward zero - //pitch *= (1.f - 5.0*frametime); - //yaw *= (1.f - 7.0*frametime); - // Update head_mouse model const float MIN_MOUSE_RATE = 30.0; const float MOUSE_SENSITIVITY = 0.1; @@ -510,7 +518,7 @@ void display(void) glTranslatef(location[0], location[1], location[2]); glPushMatrix(); - glTranslatef(WORLD_SIZE/2, WORLD_SIZE/2, WORLD_SIZE/2); + //glTranslatef(-WORLD_SIZE/2, -WORLD_SIZE/2, -WORLD_SIZE/2); int i = 0; while (i < cube_count) { glPushMatrix(); @@ -592,18 +600,25 @@ void display(void) for(i = 0; i < NUM_CHANNELS; i++) { // Actual value + glLineWidth(2.0); glColor4f(1, 1, 1, 1); glBegin(GL_LINES); glVertex2f(disp_x, HEIGHT*0.95); glVertex2f(disp_x, HEIGHT*(0.25 + 0.75f*adc_channels[i]/4096)); glEnd(); // Trailing Average value - glColor4f(0, 0, 0.8, 1); + glColor4f(1, 1, 0, 1); glBegin(GL_LINES); glVertex2f(disp_x + 2, HEIGHT*0.95); glVertex2f(disp_x + 2, HEIGHT*(0.25 + 0.75f*avg_adc_channels[i]/4096)); glEnd(); + glColor3f(1,0,0); + glBegin(GL_LINES); + glLineWidth(2.0); + glVertex2f(disp_x - 10, HEIGHT*0.5 - (adc_channels[i] - avg_adc_channels[i])); + glVertex2f(disp_x + 10, HEIGHT*0.5 - (adc_channels[i] - avg_adc_channels[i])); + glEnd(); sprintf(val, "%d", adc_channels[i]); drawtext(disp_x-GAP/2, (HEIGHT*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); @@ -653,10 +668,13 @@ void key(unsigned char k, int x, int y) } if (k == 'h') display_head = !display_head; - if (k == 'm') display_hand = !display_hand; + if (k == 'b') display_hand = !display_hand; + if (k == 'm') head_mirror = !head_mirror; + if (k == 'f') display_field = !display_field; if (k == 'l') display_levels = !display_levels; + if (k == 'e') location[1] -= WORLD_SIZE/100.0; if (k == 'c') location[1] += WORLD_SIZE/100.0; if (k == 'w') fwd_vel += 0.05; diff --git a/network.cpp b/network.cpp index e386ac913d..7cc72a53a4 100644 --- a/network.cpp +++ b/network.cpp @@ -73,7 +73,6 @@ int network_init() //from.sin_addr.s_addr = htonl(ip_address); from.sin_port = htons( (unsigned short) UDP_PORT ); - return handle; } @@ -94,7 +93,6 @@ int notify_spaceserver(int handle, float x, float y, float z) { int packet_size = strlen(data); int sent_bytes = sendto( handle, (const char*)data, packet_size, 0, (sockaddr*)&spaceserver_address, sizeof(sockaddr_in) ); - if ( sent_bytes != packet_size ) { printf( "failed to send to spaceserver: return value = %d\n", sent_bytes ); @@ -103,9 +101,11 @@ int notify_spaceserver(int handle, float x, float y, float z) { return sent_bytes; } + int network_send(int handle, char * packet_data, int packet_size) -{ - int sent_bytes = sendto( handle, (const char*)packet_data, packet_size, +{ + int sent_bytes = 0; + sent_bytes = sendto( handle, (const char*)packet_data, packet_size, 0, (sockaddr*)&dest_address, sizeof(sockaddr_in) ); if ( sent_bytes != packet_size ) diff --git a/network.h b/network.h index e06f416b9e..ea3f98cd72 100644 --- a/network.h +++ b/network.h @@ -25,6 +25,9 @@ const char DESTINATION_IP[] = "127.0.0.1"; const char SPACESERVER_IP[] = "127.0.0.1"; const int SPACESERVER_PORT = 40000; +// Randomly send a ping packet every N packets sent +const int PING_PACKET_COUNT = 20; + int network_init(); int network_send(int handle, char * packet_data, int packet_size); int network_receive(int handle, char * packet_data, int delay /*msecs*/); diff --git a/util.cpp b/util.cpp index 89fdcc8900..90dcf451ee 100644 --- a/util.cpp +++ b/util.cpp @@ -26,21 +26,23 @@ void makeCubes(float location[3], float scale, int * index, //std::cout << "loc: " << location[0] << "," //<< location[1] << "," << location[2] << "\n"; if ((*index >= MAX_CUBES) || (scale < SMALLEST_CUBE)) return; - if (randFloat() < 0.5) { + if (scale < 3 && (randFloat() < .1)) { // Make a cube - for (i = 0; i < 3; i++) cubes_position[*index*3 + i] = location[i]; + for (i = 0; i < 3; i++) cubes_position[*index*3 + i] = location[i]+scale/2.0; + float color = randFloat(); cubes_scale[*index] = scale; - cubes_color[*index*3] = randFloat(); - cubes_color[*index*3 + 1] = randFloat(); - cubes_color[*index*3 + 2] = randFloat(); + cubes_color[*index*3] = color; + cubes_color[*index*3 + 1] = color; + cubes_color[*index*3 + 2] = color; *index += 1; - //std::cout << "Quad made at scale " << scale << "\n"; + //std::cout << "Loc: " << location[0] << "," << location[1] + //<< "," << location[2] << " scale " << scale << "\n"; } else { for (i = 0; i < 8; i++) { spot[0] = location[0] + (i%2)*scale/2.0; spot[1] = location[1] + ((i/2)%2)*scale/2.0; spot[2] = location[2] + ((i/4)%2)*scale/2.0; - //std::cout << spot[0] << "," << spot[1] << "," << spot[2] << "\n"; + //std::cout << "called with " << spot[0] << "," << spot[1] << "," << spot[2] << "\n"; makeCubes(spot, scale/2.0, index, cubes_position, cubes_scale, cubes_color); } } diff --git a/world.h b/world.h index 106f47ff69..b59b19cb88 100644 --- a/world.h +++ b/world.h @@ -14,7 +14,7 @@ const float WORLD_SIZE = 10.0; #define PI 3.14159265 -#define MAX_CUBES 2000 -#define SMALLEST_CUBE 0.01 +#define MAX_CUBES 0 +#define SMALLEST_CUBE 0.1 #endif