m5stickc_rot_face

m5stickc_rot_face.ino

// Copyright (c) 2019 aNoken 
// https://anoken.jimdo.com/
// https://github.com/anoken/purin_wo_motto_mimamoru_gijutsu
// Arduino IDE compile code


#include <M5StickC.h>
int button_a = 0;
int button_b = 0;

TFT_eSprite *Spr;
uint32_t front_col = TFT_BLACK;
uint32_t back_col = TFT_YELLOW;

float rot_theta = 0.0;

float x_zero = 0;
float y_zero = 80;
float x_zero_rot = 0;
float y_zero_rot = 80;
int16_t accX = 0;
int16_t accY = 0;
int16_t accZ = 0;

int16_t gyroX = 0;
int16_t gyroY = 0;
int16_t gyroZ = 0;
int16_t temp = 0;



void rot(int16_t x_in, int16_t y_in, int16_t &x_rot, int16_t &y_rot,
         float theta) {
  x_rot = (x_in - x_zero) * cos(theta) - (y_in - y_zero) * sin(theta) + x_zero_rot;
  y_rot = (x_in - x_zero) * sin(theta) +  (y_in - y_zero) * cos(theta) + y_zero_rot;
}
/* 三角形を回転する */
void fillTriangle_r(TFT_eSPI *spi, int16_t x0, int16_t y0, int16_t x1,
                    int16_t y1, int16_t x2, int16_t y2, uint16_t color, float theta) {
  int16_t x0_rot, x1_rot, x2_rot, y0_rot, y1_rot, y2_rot;
  rot(x0, y0, x0_rot, y0_rot, theta);
  rot(x1, y1, x1_rot, y1_rot, theta);
  rot(x2, y2, x2_rot, y2_rot, theta);
  spi->fillTriangle(x0_rot, y0_rot, x1_rot, y1_rot, x2_rot, y2_rot, color);
}

/* 円形を回転する */
void fillCircle_r(TFT_eSPI *spi, int16_t x0, int16_t y0, int16_t r0,
                  uint16_t color, float theta) {
  int16_t x0_rot, y0_rot;
  rot(x0, y0, x0_rot, y0_rot, theta);
  spi->fillCircle( x0_rot, y0_rot, r0,  color);
}


/* お顔を表示する */
void draw_face(float open, float theta) {
  static long cnt = 0;
  int yy = 20;
  Spr->fillSprite(back_col);  //背景を塗る。
  fillTriangle_r(Spr, 15, 40 - yy, 60, 20 - yy, 15, 60 - yy , front_col, theta);
  fillTriangle_r(Spr, -15, 40 - yy, -60, 20 - yy, -15, 60 - yy , front_col, theta);
  fillTriangle_r(Spr, -30, 100 + 30, 30, 100 + 30 , 30, 105 + 30  , front_col, theta);
  fillTriangle_r(Spr, -30, 100 + 30, -30, 105 + 30 , 30, 105 + 30  , front_col, theta);

  if (cnt < 100) {
    fillCircle_r(Spr, 40 , 80 - yy , 27 , TFT_BLACK , theta); //左目
    fillCircle_r(Spr, 40 , 80 - yy , 25 , TFT_WHITE , theta); //左目
    fillCircle_r(Spr, 40 , 80 - yy , 15 , TFT_BLACK , theta); //左目

    fillCircle_r(Spr, -40 , 80 - yy , 27 , TFT_BLACK , theta); //左目
    fillCircle_r(Spr, -40 , 80 - yy , 25 , TFT_WHITE , theta); //左目
    fillCircle_r(Spr, -40 , 80 - yy , 15 , TFT_BLACK , theta); //左目

  }
  else if (cnt < 150) {
    fillTriangle_r(Spr, 15, 78 - yy, 65, 78 - yy, 15, 83 - yy , front_col, theta);
    fillTriangle_r(Spr, 65, 78 - yy, 65, 83 - yy, 15, 83 - yy , front_col, theta);

    fillTriangle_r(Spr, -15, 78 - yy, -65, 78 - yy, -15, 83 - yy , front_col, theta);
    fillTriangle_r(Spr, -65, 78 - yy, -65, 83 - yy, -15, 83 - yy , front_col, theta);
  }
  else {
    cnt = 0;
  }
  cnt++;
  Spr->pushSprite(0, 0);     //スプライトを表示する
}

void drawLoop(void *args) {
  Spr->setColorDepth(8);
  Spr->createSprite(320, 240);
  Spr->setBitmapColor(front_col, back_col);
  Spr->fillSprite(back_col);
  for (;;)  {
    M5.IMU.getGyroAdc(&gyroX, &gyroY, &gyroZ);
    M5.IMU.getAccelAdc(&accX, &accY, &accZ);
    M5.IMU.getTempAdc(&temp);
    rot_theta = 0.5 * rot_theta + 0.5 * (atan2(((float) accX) * M5.IMU.aRes, ((float) accY) * M5.IMU.aRes));
    float open = 0;
    draw_face( open, rot_theta);
    vTaskDelay(30 / portTICK_RATE_MS);
  }
}

void setup() {
  M5.begin();
  M5.IMU.Init();
  M5.Lcd.setRotation(0);
  Spr = new TFT_eSprite(&M5.Lcd);  //Sprite処理の初期化
  xTaskCreatePinnedToCore(drawLoop, "drawLoop", 4096, NULL, 1, NULL, 0);
  //
  pinMode(M5_BUTTON_HOME, INPUT_PULLUP);
  pinMode(M5_BUTTON_RST, INPUT_PULLUP);
  M5.Axp.ScreenBreath(0x09);
}

void loop() {
  M5.update();
  static int light_flg = 1;

  if (M5.BtnB.wasPressed()) { //Motor Rotation
    if (light_flg == 0) {
      light_flg = 1;
      M5.Axp.ScreenBreath(0x00);
    }
    else if (light_flg == 1) {
      light_flg = 0;
      M5.Axp.ScreenBreath(0x0f);
    }
  }
  static long cnt = 0;
  static bool cnt_flg = 0;
  if (M5.BtnA.wasPressed()) { //Motor Rotation
    button_b = 1;
    if (cnt == 0)  back_col = TFT_YELLOW;
    else if (cnt == 1 * 2)  back_col = 0x0120;
    else if (cnt == 2 * 2)  back_col = 0xFC9F;
    else if (cnt == 3 * 2)  back_col = 0xB7E0;
    else if (cnt == 4 * 2)  back_col = 0xFDA0;
    else if (cnt == 5 * 2)  back_col = 0xF81F;
    else if (cnt == 6 * 2)  back_col = 0xF800;
    else if (cnt == 7 * 2)  back_col = 0x07FF;
    else if (cnt == 8 * 2)  back_col = 0x07E0;
    else if (cnt == 9 * 2)  back_col = 0x001F;
    else if (cnt == 10 * 2)  back_col = 0xC618;
    else if (cnt == 11 * 2)  back_col = 0x7BE0;
    else if (cnt == 12 * 2)  back_col = 0x780F;
    else if (cnt == 13 * 2)  back_col = 0x7800;
    else if (cnt == 14 * 2)  back_col = 0x03EF;
    else if (cnt == 15 * 2)  back_col = 0x03E0;
    else if (cnt == 16 * 2)  back_col = 0x000F;
    cnt++;

    if (cnt_flg) {
      x_zero_rot = 80;
      cnt_flg = 0;
    }
    else {
      cnt_flg = 1;
      x_zero_rot = 0;
    }
    if (cnt > 34) {
      cnt = 0;
    }
  }
  vTaskDelay(30 / portTICK_RATE_MS);
}