#define ToF_ADDR 0x29//the iic address of tof
//==============IMU==========================
float accX =0.0F ;
float accY =0.0F ;
float accZ =0.0F;

float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

float pitch = 0.0F;
float roll = 0.0F;
float yaw = 0.0F;

//=============================================
int fcontact;
int i,j,k;
byte gbuf[16];
int distance;
int dis[10]={};
int disum=0;
float disave=0;
//——-Speaker———-
const int servo_pin = 26;
int freq = 50;
int ledChannel = 0;
int resolution = 10;
extern const unsigned char m5stack_startup_music[];

void setup() {
//========================IMU SETUP=============================
M5.IMU.Init();
M5.Lcd.setRotation(1);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(40, 0);
M5.Lcd.println(“IMU TEST”);
M5.Lcd.setCursor(0, 10);
M5.Lcd.println(” X Y Z”);
M5.Lcd.setCursor(0, 50);
M5.Lcd.println(” Pitch Roll Yaw”);

//============Speaker setup==============================
ledcSetup(ledChannel, freq, resolution);
ledcAttachPin(servo_pin, ledChannel);
ledcWrite(ledChannel, 256);//0°

// put your setup code here, to run once:
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(115200); // start serial for output
//Serial.println(“VLX53LOX test started.”);

//—osmar
M5.begin();

M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setTextSize(2);
//—osmar
M5.Lcd.setCursor(0, 0);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.print(“Distance: “);

}
void playMusic(const uint8_t* music_data, uint16_t sample_rate) {
uint32_t length = strlen((char*)music_data);
uint16_t delay_interval = ((uint32_t)1000000 / sample_rate);
for(int i = 0; i < length; i++) {
ledcWriteTone(ledChannel, music_data[i]*50);
delayMicroseconds(delay_interval);
}
}

void loop() {
i++;
//===============IMU =========================
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);

//============================================

distance=measure_distance();
dis[i%5]=distance;
for (j=0;j<5;j++)
{
disum=disum+dis[j];
}
disave=(float)disum/5;
disum=0;
distance=int(disave);
if(distance>500){distance=500;};
//Roll>=20deg && distance<=15cm　=>FaceContact
if (pitch>-30 &&　roll>=20 && distance<200 )
{
fcontact=100;
ledcWriteTone(ledChannel, 1250);
//delay(1000);
}

else{
fcontact=0;
ledcWriteTone(ledChannel, 0);
}
//Serial.printf(“%d,%4.3f,%4.3f,%4.3f\r\n”, distance,accX,accY,accZ);
Serial.printf(“%d,%4.3f,%4.3f,%4.3f,%d\r\n”, distance,pitch,roll,yaw,fcontact);
delay(50);

if(M5.BtnA.wasPressed())
{
esp_restart();
}
M5.update();
}

int measure_distance() {
write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);

read_block_data_at(VL53L0X_REG_RESULT_RANGE_STATUS, 12);//read 12 bytes once

uint16_t dist = makeuint16(gbuf[11], gbuf[10]);//split distance data to “dist”
byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);

//Serial.print(“distance “);
// Serial.print(dist);
//Serial.print(“,”);
//Serial.println(millis());

M5.Lcd.setCursor(0, 25);
M5.Lcd.fillRect(0, 25, 100, 25, BLACK);
M5.Lcd.print(dist);
M5.Lcd.print(“mm”);
return dist;
}

uint16_t bswap(byte b[]) {
// Big Endian unsigned short to little endian unsigned short
uint16_t val = ((b[0] << 8) & b[1]);
return val;
}

uint16_t makeuint16(int lsb, int msb) {
return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}

uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
// Converts the encoded VCSEL period register value into the real
// period in PLL clocks
uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
return vcsel_period_pclks;
}

/*
* IIC Functions
*/
/* function description: write one byte data */
void write_byte_data(byte data) {
Wire.beginTransmission(ToF_ADDR);
Wire.write(data);
Wire.endTransmission();
}

/* function description: write one byte data to specifical register */
void write_byte_data_at(byte reg, byte data) {
Wire.beginTransmission(ToF_ADDR);
Wire.write(reg);
Wire.write(data);
Wire.endTransmission();
}

/* function description: read two bytes data to specifical register */
void write_word_data_at(byte reg, uint16_t data) {
byte b0 = (data &0xFF);
byte b1 = ((data >> 8) && 0xFF);

Wire.beginTransmission(ToF_ADDR);
Wire.write(reg);
Wire.write(b0);
Wire.write(b1);
Wire.endTransmission();
}

/* function description: read one byte data */
byte read_byte_data() {
Wire.requestFrom(ToF_ADDR, 1);
while (Wire.available() < 1) delay(1);
byte b = Wire.read();
return b;
}

/* function description: read one byte data from specifical register */
byte read_byte_data_at(byte reg) {
//write_byte_data((byte)0x00);
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, 1);
while (Wire.available() < 1) delay(1);
byte b = Wire.read();
return b;
}

/* function description: read two bytes data from specifical register */
uint16_t read_word_data_at(byte reg) {
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, 2);
while (Wire.available() < 2) delay(1);
gbuf[0] = Wire.read();
gbuf[1] = Wire.read();
return bswap(gbuf);
}

/* function description: read multiple bytes data from specifical register */
void read_block_data_at(byte reg, int sz) {
int i = 0;
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, sz);
for (i=0; i<sz; i++) {
while (Wire.available() < 1) delay(1);
gbuf[i] = Wire.read();
}
}