M5Stick_SCP_1JikuAngle_rev02.ino

gistfile1.txt

#include <M5StickC.h>
#include <BluetoothSerial.h>
BluetoothSerial SerialBT;
uint64_t chipid;
char chipname[256];
//====IMU=============
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
double AngX,AngY,AngZ;
 int i,j;
int PINZ = 36;
int mV[100];
int mVMAsum=0;
int MA=10;
int mVz;
float Fz=0,Fzt=0;;
float mVsum=0,mVave=0,Fzave=0;
void setup() {
  M5.begin();
  chipid = ESP.getEfuseMac();
   sprintf( chipname, "M5StickC_%04X", (uint16_t)(chipid >> 32));
   SerialBT.begin(chipname);
   M5.IMU.Init();
  M5.Lcd.setRotation( 3 );
  M5.Lcd.setTextSize(2);
  M5.Lcd.setTextColor(0xFFE0);//Yellow
  pinMode(PINZ, ANALOG);
  mVave=0;
  mVsum=0;
}
 
void loop() {
  M5.Lcd.fillScreen(BLACK);
  i++;
  //================IMU=======================
   M5.IMU.getAccelData(&accX,&accY,&accZ);  
   if (accX>1){accX=1;}
   if (accX<-1){accX=-1;}
   if (accY>1){accY=1;}
   if (accY<-1){accY=-1;}
   if (accZ>1){accZ=1;}
   if (accZ<-1){accZ=-1;}
   AngX=asin(accX)*57.2957795130823; 
   AngY=asin(accY)*57.2957795130823;
   AngZ=asin(accZ)*57.2957795130823;
  //=========================================
  mVz=analogRead(PINZ);
  mV[i%MA]=mVz;
//==========Moving Average =============
  mVMAsum=0;
  for(j=0;j<MA;j++)
  {
    mVMAsum=mVMAsum+mV[j];
  }
  mVz=int(mVMAsum/MA);
 
//================================== 
  Fz=5.540337*mVz-10190.2 ;
  Fzt=5.540337*(mVz-mVave);
  //Serial.printf("mVz=%d,mVave=%4.0f,Fz=%4.0f,Fzave=%4.0f,Fzt=%4.0f\r\n",mVz,mVave,Fz,Fzave,Fzt );
  Serial.printf("%4.2f,%4.0f,%4.0f,%4.0f\r\n",Fzt,AngX,AngY,AngZ );
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.print(Fzt,0);
  M5.Lcd.setCursor(55, 10);
  M5.Lcd.print("gf"); 
  M5.Lcd.setCursor(0, 26);
 
  M5.Lcd.printf("AngX=%3.0f",AngX);
  M5.Lcd.setCursor(0, 42);
  M5.Lcd.printf("AngY=%3.0f",AngY);
  M5.Lcd.setCursor(0, 57);
  M5.Lcd.printf("AngZ=%3.0f",AngZ);
  
  SerialBT.print(Fzt,0);
  SerialBT.print(",");
  SerialBT.print(AngX,0);
  SerialBT.print(",");
  SerialBT.print(AngY,0);
  SerialBT.print(",");
  SerialBT.println(AngZ,0);
   
  
  SerialBT.println();
  delay(100);
  // 電源ボタンの状態取得（一度しか0以外のステータスは取得できない）
  int axpButton = M5.Axp.GetBtnPress();
  if ( axpButton == 1 ) {
    // 1秒以上電源ボタンを押している
    Serial.println("M5.Axp.GetBtnPress() == 1");
  }
  if ( axpButton == 2 ) {
    // 1秒未満電源ボタンを押して離した TARED
    //
   
     mVsum=0;
    for (i=0;i<100;i++)
    {
        mVsum=mVsum+analogRead(PINZ);
    }
    mVave=mVsum/100;
    Fzave==5.540337*mVave-10190.2;
  
     Serial.print("TATRED mVave,Fzave=");
     Serial.print(mVave);
     Serial.print(",");
     Serial.println(Fzave);
    
  }
}