Ground Sensors

I found at least two ways of using the ground sensors on the e-pucks. This is the one that works best:

  int ground_tmp[6];
  int ground[3];
  /* retrieve the values from the sensor */
  e_i2cp_init();
  e_i2cp_enable();
  e_i2cp_read(0xC0, 0);
  i = 0;
  for(j = 0; j < 6; j++) {
     if (j % 2 == 0) ground_tmp[j] = e_i2cp_read(0xC0, j + 1);
     else            ground_tmp[j] = e_i2cp_read(0xC0, j - 1);
  }
  e_i2cp_disable();

  /* getting the actual values */
  ground[0] = ((unsigned int)(ground_tmp[0] & 0xff)) +((unsigned int) ground_tmp[1] << 8) - ground_sensor_zero[id][0];
  ground[1] = ((unsigned int)(ground_tmp[2] & 0xff)) +((unsigned int) ground_tmp[3] << 8) - ground_sensor_zero[id][1];
  ground[2] = ((unsigned int)(ground_tmp[4] & 0xff)) +((unsigned int) ground_tmp[5] << 8) - ground_sensor_zero[id][2];

This is the function I use to calibrate the sensors (on black paper), store the values at the beginning or in a global variable once and for all:

void GroundSensorCalibrate() {
  int i, j;
  char tmp[50];
  long sensor_tmp[6] = {0};
  long sensor[3] = {0};
  long delay;
  
  btcomSendString("calibration ground:\n");
  for (i=0; i<16; i++) {
     // read the values
     e_i2cp_init();
     e_i2cp_enable();
     e_i2cp_read(0xC0, 0);
     for(j = 0; j < 6; j++) {
        if (j % 2 == 0) sensor_tmp[j] = e_i2cp_read(0xC0, j + 1);
        else            sensor_tmp[j] = e_i2cp_read(0xC0, j - 1);
     }
     e_i2cp_disable();
     
     // store the values
     sensor[0] += ((unsigned int)(sensor_tmp[0] & 0xff)) +((unsigned int) sensor_tmp[1] << 8);
     sensor[1] += ((unsigned int)(sensor_tmp[2] & 0xff)) +((unsigned int) sensor_tmp[3] << 8);
     sensor[2] += ((unsigned int)(sensor_tmp[4] & 0xff)) +((unsigned int) sensor_tmp[5] << 8);
     
     // pause
     for (delay = 0; delay < 100000; delay++){
        __asm__("nop");
     }
  }
  for (i=0; i<3; i++) { ground_sensor_zero[i]=(sensor[i]>>4);
     sprintf(tmp,"%d\t%d",i, ground_sensor_zero[i]);   
     btcomSendString(tmp);
     btcomSendString("\n");
  }
  
  for (delay = 0; delay < 100000; delay++){
     __asm__("nop");
  }
}