AMiRo-OS

#define window  5

int32_t counter = 0;

int32_t proxbuf[window]= { 0 };

int32_t meanDeviation(uint16_t a, uint16_t b){

  int32_t diff = a-b;

  int32_t res = 0; 

  proxbuf[counter] = (diff*100)/((a+b)/2);

  for (int i = 0; i< window; i++){

    res += proxbuf[i];

  }

  counter++;

  counter = counter % window;

  return res / window;

}

   int16_t old3 = 0;

   int16_t old4 = 0;

    old3 = prox[3];

    old4 = prox[4];

    int32_t deviation = meanDeviation(prox[3] & 0xFFF0  , prox[4] & 0xFFF0);

    // int32_t deviation = meanDeviation((prox[3]+old3) / 2  , (prox[4]+old4) / 2);

    // uint16_t notouch = 100;

    // uint16_t toucht = 20031;

    // sign = 

    // i = 0;

    chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i Deviation:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], deviation);

    BaseThread::sleep(CAN::UPDATE_PERIOD);

  }

}

int buf[10][3] = { 0 };

void shellRequestMagnetoMeter(BaseSequentialStream *chp, int argc, char *argv[]){

   int steps = 10;

  //  int i;

  // uint16_t prox[8];

  // uint32_t prox_sum = 0;

  if (argc == 1){

      chprintf(chp, "%i steps \n", atoi(argv[0]));

      steps = atoi(argv[0]);

    }else{

      chprintf(chp, "Usage: proxyRing <steps> \n");

    }

  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");

  BaseThread::sleep(MS2ST(5000));

  for (int j=0; j<steps; j++){

    // prox_sum = 0;

    // for(i=0; i<8;i++){

    //   prox[i] = global.robot.getProximityRingValue(i);

    //   prox_sum += prox[i];

    // }

    if (j < 10){

      buf[j][0] = global.hmc5883l.getMagnetizationGauss(0x00u);

      buf[j][1] = global.hmc5883l.getMagnetizationGauss(0x02u);

      buf[j][2] = global.hmc5883l.getMagnetizationGauss(0x01u);

    }

    chprintf(chp, "X:%i Y:%i Z:%i\n", global.hmc5883l.getMagnetizationGauss(0x00u), global.hmc5883l.getMagnetizationGauss(0x02u), global.hmc5883l.getMagnetizationGauss(0x01u));

void shellRequestMagnetoMeterPrint(BaseSequentialStream *chp, int argc, char *argv[]){

  for (int j=0; j<10; j++){

    chprintf(chp, "X:%i Y:%i Z:%i\n", buf[j][0], buf[j][1], buf[j][2]);

    BaseThread::sleep(CAN::UPDATE_PERIOD);

  }

}

void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv[]){

  types::position oldPos = global.odometry.getPosition();

  chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y);

}

  {"printMagnetometer", shellRequestMagnetoMeter},

  {"printMagnetometerRes", shellRequestMagnetoMeterPrint},

  {"printLocation", shellRequestPrintCoordinate},