M5Stack用超音波測距ユニットのひみつ

夏ごろに買ってどうもうまく動かなかった超音波ユニット

をどうにかしました。

結果

なんとか使えそう

(1) はずれ値を取り除いて、平均する

a.c

#include <M5StickC.h>

void setup() {
  M5.begin();
  M5.Lcd.setRotation(3);
  Wire.begin(32, 33);
}

int readEUS()
{
  uint32_t data;
  Wire.beginTransmission(0x57);
  Wire.write(0x01);
  Wire.endTransmission();
  delay(120);
  Wire.requestFrom(0x57,3);
  data  = Wire.read();data <<= 8;
  data |= Wire.read();data <<= 8;
  data |= Wire.read();
  return data / 1000;
}

int cmp_asc(const void *cmp1, const void *cmp2)
{
  int a = *((int*)cmp1);
  int b = *((int*)cmp2);
  return a - b;
}

#define HISTORY_SIZE 9
int history[HISTORY_SIZE];
int history_index = 0;
void loop() {
  int raw = readEUS();
  if(raw  <  20){ raw =   20; }
  if(1500 < raw){ raw = 1500; }

  history[history_index % HISTORY_SIZE] = raw;
  history_index++;

  int tmp[HISTORY_SIZE];
  for(int i=0; i<HISTORY_SIZE; i++){
    tmp[i] = history[i];
  }
  qsort(tmp, HISTORY_SIZE, sizeof(tmp[0]), cmp_asc);

  int sum = 0;
  int n = 0;
  for(int i=HISTORY_SIZE/3; i<2*HISTORY_SIZE/3; i++){
    sum += tmp[i];
    n++;
  }
  int avg = sum / n;

  for(int i=HISTORY_SIZE/3; i<2*HISTORY_SIZE/3; i++){
    printf("%d,", tmp[i]);
  }
  printf("%d\n", avg);

  M5.Lcd.fillScreen(TFT_BLACK);
  M5.Lcd.setCursor(3,3);
  M5.Lcd.printf("%dmm", avg);
}

(2) ケースから取り出す

ケースの丸い輪っかが悪さしている？？