Head tweaks, audio toggle

SerialInterface.cpp

@@ -58,8 +58,12 @@ int init_port(int baud)
 
 int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels)
 {
+    //  Channels: 
+    //  0, 1 = Head Pitch and Yaw 
+    //  2,3,4 = Head XYZ Acceleration
+    //
     int samples_read = 0;
-    const float AVG_RATE =  0.001;  // 0.00001;
+    const float AVG_RATE[] =  {0.001, 0.001, 0.01, 0.01, 0.01};
     char bufchar[1];
     while (read(serial_fd, bufchar, 1) > 0)
     {
@@ -72,20 +76,18 @@ 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 %d %d %d",    /* Needs to match Num Channels */
+                sscanf(serial_buffer, "%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],
+                       );
-                       &adc_channels[6],
-                       &adc_channels[7]);
                 for (int i = 0; i < NUM_CHANNELS; i++)
                 {
                     if (!first_measurement)
-                        avg_adc_channels[i] = (1.f - AVG_RATE)*avg_adc_channels[i] + 
+                        avg_adc_channels[i] = (1.f - AVG_RATE[i])*avg_adc_channels[i] + 
-                        AVG_RATE*(float)adc_channels[i];
+                        AVG_RATE[i]*(float)adc_channels[i];
                     else
                     {
                         avg_adc_channels[i] = (float)adc_channels[i];

SerialInterface.h

@@ -9,7 +9,7 @@
 int init_port (int baud);
 int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels);
 
-#define NUM_CHANNELS 8
+#define NUM_CHANNELS 5
-#define SERIAL_PORT_NAME "/dev/tty.usbmodem411"
+#define SERIAL_PORT_NAME "/dev/tty.usbmodem641"
 
 #endif

hardware/head_hand/head_hand.pde

@@ -4,16 +4,16 @@ Read a set of analog input lines and echo their readings over the serial port wi
 
 //  ADC PIN MAPPINGS 
 //
-//  0, 1 =   Head Pitch, Yaw gyro 
+//  15,16 =   Head Pitch, Yaw gyro 
-//  2,3,4    =  Head Accelerometer
+//  17,18,19    =  Head Accelerometer
-//  10,11,12 = Hand Accelerometer
 
-#define NUM_CHANNELS 8
-#define AVERAGE_COUNT 100
-#define TOGGLE_LED_SAMPLES 1000
 
-int inputPins[NUM_CHANNELS] = {0,1,2,3,4,10,11,12};
+#define NUM_CHANNELS 5
+#define MSECS_PER_SAMPLE 10
 
+int inputPins[NUM_CHANNELS] = {19,20,15,16,17};
+
+unsigned int time;
 
 int measured[NUM_CHANNELS];
 float accumulate[NUM_CHANNELS];
@@ -29,6 +29,7 @@ void setup()
     accumulate[i] = measured[i];
   }
   pinMode(BOARD_LED_PIN, OUTPUT);
+  time = millis();
 }
 
 void loop()
@@ -36,17 +37,22 @@ void loop()
   int i;
   sampleCount++;
   for (i = 0; i < NUM_CHANNELS; i++) {
-    if (sampleCount % AVERAGE_COUNT == 0) {
+    accumulate[i] += analogRead(inputPins[i]);
-      measured[i] = accumulate[i] / AVERAGE_COUNT;
+  }
+  if ((millis() - time) >= MSECS_PER_SAMPLE) {
+      time = millis();
+      for (i = 0; i < NUM_CHANNELS; i++) {
+        measured[i] = accumulate[i] / sampleCount;
         SerialUSB.print(measured[i]);
         SerialUSB.print(" ");
         accumulate[i] = 0;
-    } else {
-      accumulate[i] += analogRead(inputPins[i]);
       }  
+      //SerialUSB.print("(");
+      //SerialUSB.print(sampleCount);
+      //SerialUSB.print(")");
+      SerialUSB.println("");
+      sampleCount = 0;
   }  
-  if (sampleCount % AVERAGE_COUNT == 0) SerialUSB.println("");
-  if (sampleCount % TOGGLE_LED_SAMPLES == 0) toggleLED();
 }
 
 

head.cpp

@@ -43,6 +43,8 @@ Head::Head()
     PitchTarget = YawTarget = 0; 
     NoiseEnvelope = 1.0;
     PupilConverge = 2.1;
+    leanForward = 0.0;
+    leanSideways = 0.0;
     setNoise(0);
 }
 
@@ -51,10 +53,22 @@ void Head::reset()
     position = glm::vec3(0,0,0);
     Pitch = 0;
     Yaw = 0;
+    leanForward = leanSideways = 0;
+}
+
+//  Read the sensors
+void readSensors()
+{
+    
+}
+
+void Head::addLean(float x, float z) {
+    //  Add Body lean as impulse 
+    leanSideways += x;
+    leanForward += z;
 }
 
 //  Simulate the head over time 
-
 void Head::simulate(float deltaTime)
 {
     if (!noise)
@@ -71,6 +85,9 @@ void Head::simulate(float deltaTime)
         Roll *= (1.f - DECAY*deltaTime);
     }
     
+    leanForward *= (1.f - DECAY*30.f*deltaTime);
+    leanSideways *= (1.f - DECAY*30.f*deltaTime);
+    
     if (noise)
     {
         Pitch += (randFloat() - 0.5)*0.05*NoiseEnvelope;
@@ -118,6 +135,7 @@ void Head::render()
     glPushMatrix();
         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);

head.h

@@ -34,6 +34,8 @@ class Head {
     float MouthYaw;
     float MouthWidth;
     float MouthHeight;
+    float leanForward;
+    float leanSideways;
     
     float PitchTarget; 
     float YawTarget; 
@@ -44,6 +46,8 @@ class Head {
     
     glm::vec3 position;
     
+    void readSensors();
+    
 public:
     Head(void);
     void reset();
@@ -52,6 +56,7 @@ public:
     void setYaw(float y) {Yaw = y; }
     void addPitch(float p) {Pitch -= p; }
     void addYaw(float y){Yaw -= y; }
+    void addLean(float x, float z);
     void getPitch(float);
     void render();
     void simulate(float);

main.cpp

@@ -55,6 +55,7 @@ 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 
 
 //  Network Socket Stuff 
 //  For testing, add milliseconds of delay for received UDP packets
@@ -89,7 +90,7 @@ Hand myHand(HAND_RADIUS,
             glm::vec3(0,1,1));      //  My hand (used to manipulate things in world)
 
 glm::vec3 box(WORLD_SIZE,WORLD_SIZE,WORLD_SIZE);
-ParticleSystem balls(1000, 
+ParticleSystem balls(0, 
                      box, 
                      false,                     // Wrap?
                      0.02,                       // Noise
@@ -112,7 +113,7 @@ ParticleSystem balls(1000,
 #define RENDER_FRAME_MSECS 10
 #define SLEEP 0
 
-#define NUM_TRIS 10  
+#define NUM_TRIS 100000  
 struct {
     float vertices[NUM_TRIS * 3];
 //    float normals [NUM_TRIS * 3];
@@ -155,6 +156,7 @@ int display_field = 0;
 
 int display_head_mouse = 1;              //  Display sample mouse pointer controlled by head movement
 int head_mouse_x, head_mouse_y; 
+int head_lean_x, head_lean_y;
 
 int mouse_x, mouse_y;				//  Where is the mouse 
 int mouse_pressed = 0;				//  true if mouse has been pressed (clear when finished)
@@ -194,7 +196,7 @@ double elapsedTime;
 // 1. Add to the XCode project in the Resources/images group
 //    (ensure "Copy file" is checked
 // 2. Add to the "Copy files" build phase in the project
-char texture_filename[] = "int-texture256-v4.png";
+char texture_filename[] = "./int-texture256-v4.png";
 unsigned int texture_width = 256;
 unsigned int texture_height = 256;
 
@@ -261,8 +263,10 @@ void init(void)
 {
     int i, j; 
 
+    if (audio_on) {
         Audio::init();
         printf( "Audio started.\n" );
+    }
 
     //  Clear serial channels 
     for (i = i; i < NUM_CHANNELS; i++)
@@ -273,6 +277,8 @@ void init(void)
 
     head_mouse_x = WIDTH/2;
     head_mouse_y = HEIGHT/2; 
+    head_lean_x = WIDTH/2;
+    head_lean_y = HEIGHT/2;
     
     //  Initialize Field values 
     field_init();
@@ -346,7 +352,9 @@ void terminate () {
     // Close serial port
     //close(serial_fd);
 
+    if (audio_on) { 
         Audio::terminate();
+    }
     exit(EXIT_SUCCESS);
 }
 
@@ -421,6 +429,9 @@ void reset_sensors()
     fwd_vel = 0.0;
     head_mouse_x = WIDTH/2;
     head_mouse_y = HEIGHT/2; 
+    head_lean_x = WIDTH/2;
+    head_lean_y = HEIGHT/2; 
+    
     myHead.reset();
     myHand.reset();
     if (serial_on) read_sensors(1, &avg_adc_channels[0], &adc_channels[0]);
@@ -431,12 +442,14 @@ void update_pos(float frametime)
 {
     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[2] - avg_adc_channels[2];
+    float measured_lateral_accel = adc_channels[3] - avg_adc_channels[3];
-    float measured_fwd_accel = avg_adc_channels[3] - adc_channels[3];
+    float measured_fwd_accel = avg_adc_channels[2] - adc_channels[2];
     
     //  Update avatar head position based on measured gyro rates
-    myHead.addYaw(measured_yaw_rate * 1.20 * frametime);
+    myHead.addYaw(measured_yaw_rate * 0.25 * frametime);
-    myHead.addPitch(measured_pitch_rate * -1.0 * frametime);
+    myHead.addPitch(measured_pitch_rate * -0.25 * frametime);
+    myHead.addLean(measured_lateral_accel * frametime * 0.05, measured_fwd_accel*frametime * 0.05);
+
     //  Decay avatar head back toward zero
     //pitch *= (1.f - 5.0*frametime); 
     //yaw *= (1.f - 7.0*frametime);
@@ -456,6 +469,7 @@ void update_pos(float frametime)
     head_mouse_y = min(head_mouse_y, HEIGHT);
     
     //  Update hand/manipulator location for measured forces from serial channel
+    /*
     const float MIN_HAND_ACCEL = 30.0;
     const float HAND_FORCE_SCALE = 0.5;
     glm::vec3 hand_accel(-(avg_adc_channels[6] - adc_channels[6]),
@@ -466,6 +480,7 @@ void update_pos(float frametime)
     {
         myHand.addVel(frametime*hand_accel*HAND_FORCE_SCALE);
     }
+    */
                        
     //  Update render direction (pitch/yaw) based on measured gyro rates
     const int MIN_YAW_RATE = 300;
@@ -596,6 +611,7 @@ void display(void)
         glTexEnvf( GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE );
          
         glEnable( GL_POINT_SPRITE_ARB );
+    if (!display_head) {
         glBegin( GL_POINTS );
         {
             for (i = 0; i < NUM_TRIS; i++)
@@ -606,9 +622,9 @@ void display(void)
             }
         }
         glEnd();
-            
+        }
-        glDisable( GL_TEXTURE_2D );
         glDisable( GL_POINT_SPRITE_ARB );
+        glDisable( GL_TEXTURE_2D );
 
         //  Show field vectors
         if (display_field) field_render();
@@ -618,10 +634,10 @@ void display(void)
         if (display_hand) myHand.render();   
      
     
-        balls.render();
+        if (!display_head) balls.render();
             
         //  Render the world box 
-        render_world_box();
+        if (!display_head) render_world_box();
 
     glPopMatrix();
 
@@ -634,10 +650,10 @@ void display(void)
         glDisable(GL_LIGHTING);
 
         //drawvec3(100, 100, 0.15, 0, 1.0, 0, myHead.getPos(), 0, 1, 0);
+        glPointParameterfvARB( GL_POINT_DISTANCE_ATTENUATION_ARB, pointer_attenuation_quadratic );
 
         if (mouse_pressed == 1)
         {
-            glPointParameterfvARB( GL_POINT_DISTANCE_ATTENUATION_ARB, pointer_attenuation_quadratic );
             glPointSize( 10.0f );
             glColor3f(1,1,1);
             //glEnable(GL_POINT_SMOOTH);
@@ -648,7 +664,7 @@ void display(void)
             sprintf(val, "%d,%d", target_x, target_y); 
             drawtext(target_x, target_y-20, 0.08, 0, 1.0, 0, val, 0, 1, 0);
         }
-        if (display_head_mouse)
+        if (display_head_mouse && !display_head)
         {
             glPointSize(10.0f);
             glColor4f(1.0, 1.0, 0.0, 0.8);
@@ -737,7 +753,7 @@ void key(unsigned char k, int x, int y)
         float add[] = {0.001, 0.001, 0.001};
         field_add(add, pos);
     }
-    if (k == 't') {
+    if ((k == 't') && (audio_on)) {
         Audio::writeTone(0, 400, 1.0f, 0.5f);
     }
     if (k == '1')