Arduino pin reading nonsense

gistfile1.txt

#define XPIN 2
#define YPIN 1
#define ZPIN 0

#define MAX_POINTS 48
#define ACTUAL_POINTS 16

unsigned long readTimer = 0;
unsigned long reportTimer = 0;
unsigned long lastTimer = 0;
unsigned long now = 0;

short sampled_data[MAX_POINTS];
int smoothed_x = 0, smoothed_y = 0, smoothed_z = 0;
int sample_position = 0;
int dtw[16][16];


short gesture[MAX_POINTS];

void (*current_state)(void) = NULL;

// State list
void wait_for_training(void);
void read_training_data(void);
void wait_for_samples(void);
void store_trained_gesture(void);
void read_user_gesture(void);
void compare_gesture(void);

int readButton(int pin) {
  int button_state = !digitalRead(pin);
//  Serial.println(button_state);
//  return button_state;
  if (button_state) {
    delay(50);
  }
  return button_state & !digitalRead(pin);
}

void wait_for_training(void) {
  if (readButton(7)) {
    current_state = &read_training_data;
  }
  Serial.println("Waiting to train....");
};

void store_trained_gesture(void) {
  Serial.println("Storing...");
  store_data(gesture);
  delay(100);
  current_state = &wait_for_samples;
}

void store_data(short *dst) {
  memcpy(sampled_data, dst, sizeof(short) * MAX_POINTS);
  memset(sampled_data, 0, sizeof(short) * MAX_POINTS);
}

int smooth(int data, float filter, int smoothed) {  
  return ((data * (1-filter)) + (smoothed * filter));
}

int sample_data(void) {
  smoothed_x = smooth(analogRead(XPIN), 0.8, smoothed_x);
  smoothed_y = smooth(analogRead(YPIN), 0.8, smoothed_y);
  smoothed_z = smooth(analogRead(ZPIN), 0.8, smoothed_z);
  int x, y, z;
  char buf[32];
  
  unsigned long now = millis();
  if (now - lastTimer > 33) {
    x = map(smoothed_x, 0, 1023, -16, 16);    
    y = map(smoothed_y, 0, 1023, -16, 16);    
    z = map(smoothed_z, 0, 1023, -16, 16);    

    sprintf(buf, "%d %d %d", x, y, z);
    Serial.println(buf);
    /*
    sampled_data[sample_position*3] = map(smoothed_x, -16, 16, 0, 1023);    
    sampled_data[sample_position*3+1] = map(smoothed_y, -16, 16, 0, 1023);    
    sampled_data[sample_position*3+2] = map(smoothed_z, -16, 16, 0, 1023);    
    
    sample_position++;
    
    if (sample_position >= ACTUAL_POINTS) {
      sample_position = 0;      
      return -1;
    }
    */   
    lastTimer = now;
  }
  
  return 0;
};

void read_training_data(void) {
  if (digitalRead(7)) {
    current_state = &wait_for_samples;
    Serial.println("END");
    return;
  }
  
  // Sample the data until we get a -1, then state transition
  if (sample_data() == -1) {
    Serial.println("END");
    current_state = &wait_for_samples;
  }
//  Serial.println("Receiving training data");
}

void wait_for_samples(void) {  
  if (readButton(7)) {
    memset(sampled_data, 0, sizeof(sampled_data[0]) * MAX_POINTS);
    Serial.println("BEGIN");    
    current_state = &read_user_gesture;
  }
}

void read_user_gesture(void) {
  if (digitalRead(7)) {
    current_state = &wait_for_samples;
  }
  
  // Sample the data until we get a -1, then state transition
  if (sample_data() == -1) {
    current_state = &wait_for_samples;
  }
 
  //Serial.println("Receiving gesture data");
}

void compare_gesture(void) {
  Serial.println("Comparing! Success!");
  int d = uWaveDistance(sampled_data, gesture);
  Serial.println(d);
  delay(2000);
  current_state = &wait_for_samples;
};

void setup()
{
  Serial.begin(9600);
  lastTimer = millis();
  readTimer = millis();
  reportTimer = millis();
  analogReference(EXTERNAL);
  pinMode(7, INPUT);
  digitalWrite(7, HIGH);
  current_state = &wait_for_samples;  
  
  // clear the sample
  for (int i = 0; i < MAX_POINTS; i++) {
    sampled_data[i] = 0;
    gesture[i] = 0;    
  }
}

void loop() {
  //Serial.println(digitalRead(7));
  (*current_state)();
}

int avg(unsigned int *numArr, int len) {
  unsigned int tmp = 0;
  for (int i = 0; i < len; i++) {
    tmp += numArr[i];
  }
  return (int) (tmp/len);
}

int vector_distance(int x1, int y1, int z1, int x2, int y2, int z2) {
  int xdel = x2 - x1;
  int ydel = y2 - y1;
  int zdel = z2 - z1;
  int ret = xdel * xdel + ydel * ydel + zdel * zdel;
  if (ret == 0) {
    Serial.println("RETURNED 0");
  }
  Serial.println(ret);
}

// uWave version of DTW, assumes the sequence is an array of vector triplets
// Also assumes uses euclidean distance between the vectors
// The sequence is actually 3*len1
int uWaveDistance(short *seq1, short *seq2) {
  Serial.println("Point 1");
  int cost = 0;
  int x1, y1, z1, x2, y2, z2;
  
  

  // Set up the array
  for (int i = 0; i < ACTUAL_POINTS; i++) {
    for (int j = 0; j < ACTUAL_POINTS; j++) {
      dtw[i][j] = 32767; // Maxint
    }
  }
  dtw[0][0] = vector_distance(seq1[0], seq1[1], seq1[2], seq2[0], seq2[1], seq2[3]);
  Serial.println(4);  
  Serial.println(dtw[0][0]);  
  // Set up the timewarp
  for (int i = 0; i < ACTUAL_POINTS; i++) {
    for (int j = 0; j < ACTUAL_POINTS; j++) {
      if (i == 0 && j== 0) {
        continue;
      }
      x1 = seq1[i*3];
      y1 = seq1[i*3+1];
      z1 = seq1[i*3+2];
      x2 = seq2[j*3];
      y2 = seq2[j*3+1];
      z2 = seq2[j*3+2];
      cost = vector_distance(x1, y1, z1, x2, y2, z2);
      dtw[i][j] = cost + min(dtw[i][j-1], min(dtw[i-1][j-1], dtw[i-1][j]));
      Serial.println(dtw[i][j]);
    }
  }

  return dtw[ACTUAL_POINTS-1][ACTUAL_POINTS-1];
}