SHISEIGYO2_02

SHISEIGYO2_02.ino

#include <M5Core2.h>
#include <Preferences.h>
#include <Fonts/EVA_20px.h>

#include <stdio.h>
#include <Kalman.h> // https://github.com/TKJElectronics/KalmanFilter


#define ENCx_A 35
#define ENCx_B 36
#define brakeX 0
#define rote_pinX 2
#define PWM_pinX 26

#define ENCy_A 33
#define ENCy_B 32
#define brakeY 13
#define rote_pinY 14
#define PWM_pinY 25

#define CH_X 0
#define CH_Y 1

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

volatile byte posX, posY;
volatile int  enc_countX = 0, enc_countY = 0;

float Kp = 10;
float Kd = 10;
float Kw = 0.7;
float IDRS = 2;
int rotMax = 370;
float getupRange = 0.1;
float injectRatio = 80.0;

int pwmDutyX, pwmDutyY;

int GetUP = 0;
int GetUpcnt = 0;
int cnt = 99;

float Mx, My;
float AjX = 0.0, AjY = 0.0;
float rot = 0.0, rotX = 0.0, rotY = 0.0;


float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;

float theta_X = 0.0, theta_Y = 0.0;
float theta_Xdot = 0.0, theta_Ydot = 0.0;

//オフセット
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;

Kalman kalmanX, kalmanY;
float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;

static TFT_eSprite *sprite = nullptr;
static bool IMURunning = false;

int scene = 0;

Preferences preferences;

//センサオフセット算出
void offset_cal(){
  delay(300);
  M5.Lcd.fillScreen(TFT_RED);

  delay(300);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;

  for(int i=0; i<10; i++) {
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(50);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  accXoffset = accXoffset / 10 + 1.0 / sqrt(2.0);
  accYoffset = accYoffset / 10 + 1.0 / sqrt(2.0);
  accZoffset /= 10.0;
  gyroXoffset /= 10.0;
  gyroYoffset /= 10.0;
  gyroZoffset /= 10.0;

  M5.Lcd.fillScreen(TFT_BLACK);
}

//加速度センサから傾きデータ取得 [deg]
void get_theta() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_X  = atan2(+1.0 * (accY - accYoffset) , (accZ - accZoffset)) * 180.0/PI;
  theta_Y  = atan2(-1.0 * (accX - accXoffset) , (accZ - accZoffset)) * 180.0/PI;
}

//角速度取得
void get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_Xdot = gyroX - gyroXoffset;
  theta_Ydot = gyroY - gyroYoffset;
}

//起き上がり
void getUp() {
  Serial.println("getUp!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
  if(kalAngleX < -14.0 && kalAngleX > -24.0 && kalAngleY > 14.0 && kalAngleY < 24.0){
    M5.Axp.SetLDOEnable(3, true);
    delay(500);
    M5.Axp.SetLDOEnable(3, false);
    
    GetUP = 1;
     
    AjX = 0.0;
    AjY = 0.0;

    digitalWrite(brakeX, HIGH);
    digitalWrite(brakeY, HIGH);

    digitalWrite(rote_pinX, HIGH);
    digitalWrite(rote_pinY, LOW);

    for(int i = 1023; i >= rotMax; i--){
      ledcWrite(CH_X, i);
      ledcWrite(CH_Y, i);
      cnt = map(i, 1023, rotMax, 3, 0);
      Serial.println(cnt);
      delay(5);
    }

    ledcWrite(CH_X, rotMax);
    ledcWrite(CH_Y, rotMax);
    delay(300);
    scene = 0;
    cnt = 99;
    digitalWrite(rote_pinX, LOW);
    digitalWrite(rote_pinY, HIGH);
  }
}

void ENCx_READ() {
  byte cur = (!digitalRead(ENCx_B) << 1) + !digitalRead(ENCx_A);
  byte old = posX & B00000011;
  byte dir = (posX & B00110000) >> 4;

  if (cur == 3) cur = 2;
  else if (cur == 2) cur = 3;

  if (cur != old)
  {
    if (dir == 0)
    {
      if (cur == 1 || cur == 3) dir = cur;
    } else {
      if (cur == 0)
      {
        if (dir == 1 && old == 3) enc_countX--;
        else if (dir == 3 && old == 1) enc_countX++;
        dir = 0;
      }
    }

    bool rote = 0;
    if (cur == 3 && old == 0) rote = 0;
    else if (cur == 0 && old == 3) rote = 1;
    else if (cur > old) rote = 1;

    posX = (dir << 4) + (old << 2) + cur;
  }
}


void ENCy_READ() {
  byte cur = (!digitalRead(ENCy_B) << 1) + !digitalRead(ENCy_A);
  byte old = posY & B00000011;
  byte dir = (posY & B00110000) >> 4;

  if (cur == 3) cur = 2;
  else if (cur == 2) cur = 3;

  if (cur != old)
  {
    if (dir == 0)
    {
      if (cur == 1 || cur == 3) dir = cur;
    } else {
      if (cur == 0)
      {
        if (dir == 1 && old == 3) enc_countY--;
        else if (dir == 3 && old == 1) enc_countY++;
        dir = 0;
      }
    }

    bool rote = 0;
    if (cur == 3 && old == 0) rote = 0;
    else if (cur == 0 && old == 3) rote = 1;
    else if (cur > old) rote = 1;

    posY = (dir << 4) + (old << 2) + cur;
  }
}



//ディスプレイ表示 Core0
void displayLoop(void *pvParameters) {

  disableCore0WDT(); // only needs to be called once

  sprite = new TFT_eSprite((M5Display*)pvParameters);
  
  // sprite->setPsram( false ); // faster rendering without psram but requires to patch sprite.cpp
  sprite->setColorDepth( 8 ); // however use 8 bits colors because there isn't enough ram for 16 bits
  sprite->setFreeFont( nullptr ); // for some reason sprites aren't initiated with a font
  sprite->setTextColor( TFT_WHITE );
  sprite->createSprite( 320, 240 );
  
  uint32_t colorX, colorY;
  int xx, yy, lastxx =-1, lastyy =-1;
  char Str[32];
  
  while( !IMURunning ) {
    // wait until the other task has fully started
    vTaskDelay(1);
  }
  
  while(1) {
    vTaskDelay(1);

    //Main Display
    if(scene == 0){
      xx = map( kalAngleX, 15, -15, 0, 240 );
      yy = map( kalAngleY, 20, -20, 0, 320 );
  
      if( xx == lastxx && yy == lastyy ) {
        // nothing to draw
        continue;
      }
  
      sprite->fillSprite( TFT_BLACK );
      
      if(fabs(kalAngleX) <= 1.0){
        colorX = GREEN;
      } else {
        colorX = RED;
      }
  
      if(fabs(kalAngleY) <= 1.0){
        colorY = GREEN;
      } else {
        colorY = BLUE;
      }
  
      sprite->fillRect( 0, xx - 2, 320, 5, colorX );
      sprite->fillRect( yy - 2, 0, 5, 240, colorY );
      
      lastxx = xx;
      lastyy = yy;


      sprite->drawRoundRect(0, 25, 155, 37, 5, WHITE);
      sprite->drawRoundRect(0, 75, 155, 37, 5, WHITE);
      sprite->drawRoundRect(0, 125, 155, 37, 5, WHITE);

      sprite->drawRoundRect(165, 25, 155, 37, 5, WHITE);
      sprite->drawRoundRect(165, 75, 155, 37, 5, WHITE);
      sprite->drawRoundRect(165, 125, 155, 37, 5, WHITE);
      sprite->drawRoundRect(165, 175, 155, 37, 5, WHITE);
      
      sprite->setTextSize(2);
      snprintf( Str, 32, "X:%.2f", kalAngleX );
      sprite->drawString( Str, 10, 0 );
      snprintf( Str, 32, "Y:%.2f", kalAngleY );
      sprite->drawString(Str, 140, 0);

      sprite->setTextSize(3);
      snprintf( Str, 32, "rMax:%d", rotMax );
      sprite->drawString(Str, 7, 33);
      snprintf( Str, 32, "GR:%.1f", getupRange );
      sprite->drawString(Str, 7, 83);
      snprintf( Str, 32, "IR:%.0f", injectRatio );
      sprite->drawString(Str, 7, 133);
      
  
      snprintf( Str, 32, "Kp:%.1f", Kp );
      sprite->drawString( Str, 170, 33 );
  
      snprintf( Str, 32, "Kd:%.1f", Kd );
      sprite->drawString( Str, 170, 83 );
  
      snprintf( Str, 32, "Kw:%.1f", Kw );
      sprite->drawString( Str, 170, 133 );
  
      snprintf( Str, 32, "IDRS:%.1f", IDRS );
      sprite->drawString( Str, 170, 183 );

  
      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "GetUP" );
      sprite->drawString( Str, 25, 224 );
  
      sprite->pushSprite( 0, 0 );
    }

    //rotMax
    if(scene == 1){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "rotMax" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Maximum rotation speed" );
      sprite->drawString( Str, 20, 40 );
      snprintf( Str, 32, "%s", "when getting up" );
      sprite->drawString( Str, 20, 56 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%d", rotMax );
      sprite->drawString(Str, 125, 120);
    }

    //getupRange
    if(scene == 2){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "getupRange" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Angle to stop getting up" );
      sprite->drawString( Str, 20, 40 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.1f", getupRange );
      sprite->drawString(Str, 125, 120);
    }

    //injectRatio
    if(scene == 3){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "injectRatio" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Ratio of angle and" );
      sprite->drawString( Str, 20, 40 );
      snprintf( Str, 32, "%s", "rotation when getting up" );
      sprite->drawString( Str, 20, 56 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.0f", injectRatio );
      sprite->drawString(Str, 125, 120);
    }

    //Kp
    if(scene == 4){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "Kp" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Coefficient for module angle" );
      sprite->drawString( Str, 20, 40 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.1f", Kp );
      sprite->drawString(Str, 125, 120);
    }

    //Kd
    if(scene == 5){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "Kd" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Coefficient for " );
      sprite->drawString( Str, 20, 40 );
      snprintf( Str, 32, "%s", "module angular velocity" );
      sprite->drawString( Str, 20, 56 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.1f", Kd );
      sprite->drawString(Str, 125, 120);
    }

    //Kw
    if(scene == 6){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "Kw" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Coefficient for " );
      sprite->drawString( Str, 20, 40 );
      snprintf( Str, 32, "%s", "wheel rotation speed" );
      sprite->drawString( Str, 20, 56 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.1f", Kw );
      sprite->drawString(Str, 125, 120);
    }

    //IDRS
    if(scene == 7){
      sprite->fillSprite( TFT_BLACK );

      sprite->setTextSize(3);
      snprintf( Str, 32, "%s", "IDRS" );
      sprite->drawString( Str, 10, 10 );

      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", "Inverted angle Dynamic" );
      sprite->drawString( Str, 20, 40 );
      snprintf( Str, 32, "%s", "Readjustment System" );
      sprite->drawString( Str, 20, 56 );

      sprite->setTextSize(4);
      snprintf( Str, 32, "%.1f", IDRS );
      sprite->drawString(Str, 125, 120);
    }

    //GetUp
    if(scene == 8){
      if(cnt == 0){
        sprite->fillSprite( TFT_WHITE );
      }else{
        sprite->fillSprite( TFT_BLACK );
      }
      sprite->setTextSize(7);
      if(cnt == 3){
        snprintf( Str, 32, "%s", "3" );
        sprite->drawString(Str, 135, 95);
      }
      if(cnt == 2){
        snprintf( Str, 32, "%s", "2" );
        sprite->drawString(Str, 135, 95);
      }
      if(cnt == 1){
        snprintf( Str, 32, "%s", "1" );
        sprite->drawString(Str, 135, 95);
      }
      sprite->pushSprite( 0, 0 );
    }

    if(scene != 0 && scene != 8){
      sprite->setTextSize(2);
      snprintf( Str, 32, "%s", " - " );
      sprite->drawString( Str, 35, 224 );
  
      snprintf( Str, 32, "%s"," + " );
      sprite->drawString( Str, 142, 224 );
  
      snprintf( Str, 32, "%s", "Back" );
      sprite->drawString( Str, 243, 224 );
  
      sprite->pushSprite( 0, 0 );
    }
  }

  sprite->deleteSprite();
  vTaskDelete( NULL );
}




void IMULoop( void * param ) {  
  M5.IMU.Init();

  //フルスケールレンジ
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G);
  M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS);
  
  pinMode(rote_pinX, OUTPUT);
  pinMode(brakeX, OUTPUT);
  digitalWrite(brakeX, LOW);
  
  pinMode(rote_pinY, OUTPUT);
  pinMode(brakeY, OUTPUT);
  digitalWrite(brakeY, LOW);

  pinMode(ENCx_A, INPUT);
  pinMode(ENCx_B, INPUT);
  pinMode(ENCy_A, INPUT);
  pinMode(ENCy_B, INPUT);

  attachInterrupt(ENCx_A, ENCx_READ, CHANGE);
  attachInterrupt(ENCx_B, ENCx_READ, CHANGE);
  attachInterrupt(ENCy_A, ENCy_READ, CHANGE);
  attachInterrupt(ENCy_B, ENCy_READ, CHANGE);

  ledcSetup(CH_X, 20000, 10);
  ledcAttachPin(PWM_pinX, CH_X);
  ledcSetup(CH_Y, 20000, 10);
  ledcAttachPin(PWM_pinY, CH_Y);

  delay(500);

  //センサオフセット算出
  offset_cal();

  get_theta();
  kalmanX.setAngle(theta_X);
  kalmanY.setAngle(theta_Y);

  IMURunning = true;

  while(1) {
    nowTime = micros();
    loopTime = nowTime - oldTime;
    oldTime = nowTime;

    dt = (float)loopTime / 1000000.0; //sec

    //モータの角速度算出
    float theta_XdotWheel = -1.0 * float(enc_countX) * 3.6 / dt;
    enc_countX = 0;
    float theta_YdotWheel = -1.0 * float(enc_countY) * 3.6 / dt;
    enc_countY = 0;

    get_theta();
    get_gyro_data();

    //カルマンフィルタ 姿勢 傾き
    kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt);
    kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt);

    //カルマンフィルタ 姿勢 角速度
    kalAngleDotX = kalmanX.getRate();
    kalAngleDotY = kalmanY.getRate();

    //タッチボタン
    TouchPoint_t pos = M5.Touch.getPressPoint();
    if(pos.y > 240 && pos.x < 106 && GetUP != 99) {
      switch(scene){
        case 0: //Main
          scene = 8;
          getUp();
          break;
        case 1: //rotMax
          if(rotMax > 0) rotMax -= 10;
          delay(200);
          break;
        case 2: //getupRange
          if(getupRange > -1.0) getupRange -= 0.1;
          delay(200);
          break;
        case 3: //injectRatio
          if(injectRatio > 0) injectRatio -= 2;
          delay(200);
          break;
        case 4: //Kp
          if(Kp > 0) Kp -= 0.1;
          delay(200);
          break;
        case 5: //Kd
          if(Kd > 0) Kd -= 0.1;
          delay(200);
          break;
        case 6: //Kw
          if(Kw > 0) Kw -= 0.1;
          delay(200);
          break;
        case 7: //IDRS
          if(IDRS > 0) IDRS -= 0.1;
          delay(200);
          break;
      }
    }else if(pos.y > 240 && pos.x < 213){
      switch(scene){
        case 1: //rotMax
          if(rotMax < 900) rotMax += 10;
          delay(200);
          break;
        case 2: //getupRange
          if(getupRange < 1.0) getupRange += 0.1;
          delay(200);
          break;
        case 3: //injectRatio
          if(injectRatio < 200) injectRatio += 2;
          delay(200);
          break;
        case 4: //Kp
          if(Kp < 20) Kp += 0.1;
          delay(200);
          break;
        case 5: //Kd
          if(Kd < 20) Kd += 0.1;
          delay(200);
          break;
        case 6: //Kw
          if(Kw < 20) Kw += 0.1;
          delay(200);
          break;
        case 7: //IDRS
          if(IDRS < 10) IDRS += 0.1;
          delay(200);
          break;
      }
    }else if(pos.y > 240 && pos.x < 320){
      if(scene != 0){
        switch(scene){ //パラメータ保存
          case 1: //rotMax
            preferences.putInt("rotMax", rotMax);
            break;
          case 2: //getupRange
            preferences.putFloat("getupRange", getupRange);
            break;
          case 3: //injectRatio
            preferences.putFloat("injectRatio", injectRatio);
            break;
          case 4: //Kp
            preferences.putFloat("Kp", Kp);
            break;
          case 5: //Kd
            preferences.putFloat("Kd", Kd);
            break;
          case 6: //Kw
            preferences.putFloat("Kw", Kw);
            break;
          case 7: //IDRS
            preferences.putFloat("IDRS", IDRS);
            break;
        }
        scene = 0;
        M5.Axp.SetLDOEnable(3, true);
        delay(300);
        M5.Axp.SetLDOEnable(3, false);
      }
    }

    //パラメータボタン
    if(GetUP != 99){
      //rotMax
      if(pos.x < 165 && pos.y > 25 && pos.y < 75 && scene == 0) {
        scene = 1;
      }
      //getupRange
      if(pos.x < 165 && pos.y > 75 && pos.y < 125 && scene == 0) {
        scene = 2;
      }
      //injectRatio
      if(pos.x < 165 && pos.y > 125 && pos.y < 175 && scene == 0) {
        scene = 3;
      }
      //Kp
      if(pos.x > 165 && pos.y > 25 && pos.y < 75 && scene == 0) {
        scene = 4;
      }
      //Kd
      if(pos.x > 165 && pos.y > 75 && pos.y < 125 && scene == 0) {
        scene = 5;
      }
      //Kw
      if(pos.x > 165 && pos.y > 125 && pos.y < 175 && scene == 0) {
        scene = 6;
      }
      //IDRS
      if(pos.x > 165 && pos.y > 175 && pos.y < 225 && scene == 0) {
        scene = 7;
      }
    }
    

    //起き上がり動作
    if(GetUP == 1){
      if(kalAngleX >= getupRange && kalAngleY <= -getupRange){
        digitalWrite(brakeX, LOW);
        digitalWrite(brakeY, LOW);
        GetUP = 99;
      }else{
        if(kalAngleX < 0){
          ledcWrite(CH_X, max(1023 - int(injectRatio * fabs(kalAngleX)), 0));
        }
        if(kalAngleY > 0){
          ledcWrite(CH_Y, max(1023 - int(injectRatio * fabs(kalAngleY)), 0));
        }
      }
    }
       
    if ((fabs(kalAngleX) < 1 && fabs(kalAngleY) < 1) && GetUP == 0 ){
      GetUP = 99;
    }
    
    if(GetUP == 99){
      //ブレーキ
      if(fabs(kalAngleX) > 14.0 || fabs(kalAngleY) > 14.0){
        digitalWrite(brakeY, LOW);
        digitalWrite(brakeX, LOW);
        GetUP = 0;
        AjX = 0.0;
        AjY = 0.0;
      }else{
        digitalWrite(brakeX, HIGH);
        digitalWrite(brakeY, HIGH);
      }
     
      
      //モータ回転
      AjX +=  IDRS * theta_XdotWheel / 1000000.0;
      AjY +=  IDRS * theta_YdotWheel / 1000000.0;
      
      Mx = Kp * (kalAngleX + AjX) / 90.0 + Kd * kalAngleDotX / 500.0 + Kw * theta_XdotWheel / 10000.0;
      My = Kp * (kalAngleY + AjY) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_YdotWheel / 10000.0;
    
      Mx = max(-1.0f, min(1.0f, Mx));
      pwmDutyX = 1023 * (1.0 - fabs(Mx));
      My = max(-1.0f, min(1.0f, My));
      pwmDutyY = 1023 * (1.0 - fabs(My));
  
      //回転方向 
      if(pwmDutyX > 1000){
        digitalWrite(brakeX, LOW);
        ledcWrite(CH_X, 1023);
      }else if(Mx > 0.0){
        digitalWrite(rote_pinX, HIGH);
        ledcWrite(CH_X, pwmDutyX);
      }else{
        digitalWrite(rote_pinX, LOW);
        ledcWrite(CH_X, pwmDutyX);
      }
  
      if(pwmDutyY > 1000){
        digitalWrite(brakeY, LOW);
        ledcWrite(CH_Y, 1023);
      }else if(My < 0.0){
        digitalWrite(rote_pinY, LOW);
        ledcWrite(CH_Y, pwmDutyY);
      }else{
        digitalWrite(rote_pinY, HIGH);
        ledcWrite(CH_Y, pwmDutyY);
      }
    }
      
    Serial.print("loopTime: ");
    Serial.print((float)loopTime / 1000.0);

    Serial.println("");

    //vTaskDelay(1);
    if(GetUP == 99) {
      delay(6);
    }else{
      delay(1);
    }
  }

  vTaskDelete( NULL );
}



void setup() {
  M5.begin(true, true, true, true); //LCD, SD, Serial, I2C
  M5.Lcd.fillScreen(TFT_BLACK);

  preferences.begin("parameter", false);

  //パラメータ初期値取得
  rotMax = preferences.getInt("rotMax", rotMax);
  getupRange = preferences.getFloat("getupRange", getupRange);
  injectRatio = preferences.getFloat("injectRatio", injectRatio);
  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);
  Kw = preferences.getFloat("Kw", Kw);
  IDRS = preferences.getFloat("IDRS", IDRS);

  enum CPUCores {
    ESP32_CORE0 = 0,
    ESP32_CORE1 = 1
  };

  //ディスプレイ表示 タスク
  //xTaskCreatePinnedToCore( task, "task", stacksize, &object, (int)priority, taskhandle, (int)core );
  xTaskCreatePinnedToCore( IMULoop,     "IMULoop",     4096, NULL,    2,  NULL, ESP32_CORE1 ); // I2C prefers core 1
  xTaskCreatePinnedToCore( displayLoop, "displayLoop", 4096, &M5.Lcd, 1,  NULL, ESP32_CORE0 ); // LCD will run on core 0

}



void loop() {
  // don't put anything else in there
  vTaskDelete( NULL );
}