m5stickcplus_sand_demo.ino

#include <Wire.h>
#include <bb_spi_lcd.h>


#define WIDTH 240
#define HEIGHT 135

#define TFT_CS 5
#define TFT_RST 18
#define TFT_DC 23
#define TFT_CLK 13
#define TFT_MOSI 15

#define RTC_SDA_PIN 21
#define RTC_SCL_PIN 22
#define IMU_ADDR 0x68
#define AXP_ADDR 0x34

#define BUTTON_A 37
#define BUTTON_B 39
#define BUTTON_ACTIVE LOW
#define BUTTON_A_CHANGED 1
#define BUTTON_B_CHANGED 2

static SPILCD lcd;
static int ButtA, ButtB, oldButtA, oldButtB;
static uint8_t ucTXBuf[2048];

static int GetButtons(void)
{
int rc;
   ButtA = digitalRead(BUTTON_A);
   ButtB = digitalRead(BUTTON_B);
   rc = (ButtA != oldButtA) | ((ButtB != oldButtB) << 1);
//   if (rc && iBeep) {
//    if ((oldButtA != BUTTON_ACTIVE && ButtA == BUTTON_ACTIVE) ||
//       (oldButtB != BUTTON_ACTIVE && ButtB == BUTTON_ACTIVE))
//       Beep(4000); // quick beep for a button press
//       delay(50);
//       Beep(0);
//   }
   oldButtA = ButtA;
   oldButtB = ButtB;
   return rc;
} /* GetButtons() */

int IMURead(int16_t *px, int16_t *py, int16_t *pz)
{
uint8_t ucTemp[8];
int x, y, z;
uint8_t start_reg, len;

  start_reg = 0x3b;
  len = 6;

  x = I2CReadRegister(IMU_ADDR, start_reg, ucTemp, len);
  if (x > 0)
  {
    x = (ucTemp[0] << 8) + ucTemp[1];
    y = (ucTemp[2] << 8) + ucTemp[3];
    z = (ucTemp[4] << 8) + ucTemp[5];
    if (x > 32767) x -= 65536; // two's compliment
    if (y > 32767) y -= 65536;
    if (z > 32767) z -= 65536;
    *px = (int16_t)x;
    *py = (int16_t)y;
    *pz = (int16_t)z;
    return 0;
  }
  return -1;
} /* IMURead() */

void I2CRegisterWrite( uint8_t Addr , uint8_t Register, uint8_t Data)
{
    Wire1.beginTransmission(Addr);
    Wire1.write(Register);
    Wire1.write(Data);
    Wire1.endTransmission();
}

void I2CWrite( uint8_t Addr , uint8_t *Data, int iLen )
{
    Wire1.beginTransmission(Addr);
    while (iLen) {
       Wire1.write(*Data++);
       iLen--;
    }
    Wire1.endTransmission();
}

uint8_t I2CReadByte( uint8_t Addr , uint8_t Register)
{
    Wire1.beginTransmission(Addr);
    Wire1.write(Register);
    Wire1.endTransmission();
    Wire1.requestFrom(Addr, (uint8_t)1);
    return Wire1.read();
}

uint8_t I2CReadRegister(uint8_t Addr, uint8_t start_reg, uint8_t *Data, uint8_t len)
{
  uint8_t oldlen = len;
   Wire1.beginTransmission(Addr);
   Wire1.write(start_reg); 
   Wire1.endTransmission();
   
   Wire1.requestFrom(Addr, len);
   while (Wire1.available() > 0 && len) {
    *Data++ = Wire1.read();
    len--;
   }
   return (oldlen - len);
} /* I2CReadRegister() */

static void IMUInit(void)
{
unsigned char ucTemp[4];

            ucTemp[0] = 0x6b; // PWR_MGMT_1
            ucTemp[1] = 0x80; // reset chip
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(10);
            ucTemp[1] = 1; // select the best available oscillator
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(10);
            ucTemp[0] = 0x1c; // ACCEL_CONFIG
            ucTemp[1] = 0x00; // full scale = 2G, all axes enabled
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x1b; // GYRO_CONFIG
            ucTemp[1] = 0x18; // +/- 2000 degrees per second
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x1a; // CONFIG
            ucTemp[1] = 1; // 176 filtered samples per sec (1k sampling rate)
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x19; // SMPLRT_DIV
            ucTemp[1] = 0x05; // sample rate divider (1000 / (1+this_val))
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x38; // INT_ENABLE
            ucTemp[1] = 0x00; // disable interrupts
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x1d; // ACCEL_CONFIG2
            ucTemp[1] = 0x00; // avg 4 samples
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x6a; // USER_CTRL
            ucTemp[1] = 0x00; // disable FIFO
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x23; // FIFO_EN
            ucTemp[1] = 0x00; // disable
            I2CWrite(IMU_ADDR, ucTemp, 2);
            delay(1);
            ucTemp[0] = 0x37; // INT_PIN_CFG
            ucTemp[1] = 0x22; // latch int enable
            I2CWrite(IMU_ADDR, ucTemp, 2);
//            delay(1);
//            ucTemp[0] = 0x38; // INT_ENABLE
//            ucTemp[1] = 0x01; // enable interrupt on data ready
//            I2CWrite(IMU_ADDR, ucTemp, 2);

} /* IMUInit() */

// Number of grains of sand - this is how many pixels in the first line of text
#define N_GRAINS     720
// The 'sand' grains exist in an integer coordinate space that's 256X
// the scale of the pixel grid, allowing them to move and interact at
// less than whole-pixel increments.
#define MAX_X (WIDTH  * 256 - 1) // Maximum X coordinate in grain space
#define MAX_Y (HEIGHT * 256 - 1) // Maximum Y coordinate
struct Grain {
  uint16_t  x,  y; // Position
  int16_t vx, vy; // Velocity
  uint16_t color; // pixel color
} grain[N_GRAINS];

void ResetGrains(int bRandom)
{
int i, j, x, y;
uint16_t *pBitmap = spilcdGetBuffer(&lcd);
uint16_t color, Pal[] = {0xf800,0xffff,0xffe0,0xf81f,0x1f,0x6e0,0x6ff,0xaaaa};

  spilcdFill(&lcd, 0, DRAW_TO_LCD | DRAW_TO_RAM);
  if (bRandom)
  {
    for(i=0; i<N_GRAINS; i++) {  // For each sand grain...
      do {
        grain[i].x = random(WIDTH  * 256); // Assign random position within
        grain[i].y = random(HEIGHT * 256); // the 'grain' coordinate space
        // Check if corresponding pixel position is already occupied...
        for(j=0; (j<i) && (((grain[i].x / 256) != (grain[j].x / 256)) ||
                           ((grain[i].y / 256) != (grain[j].y / 256))); j++);
      } while(j < i); // Keep retrying until a clear spot is found
      x = grain[i].x / 256; y = grain[i].y / 256;
      // because the display is rotated 90
      grain[i].vx = grain[i].vy = 0; // Initial velocity is zero
      grain[i].color = Pal[random(7)];
      pBitmap[x + (y*WIDTH)] = grain[i].color; // Mark it
    }
  } // random
  else
  {
    spilcdWriteString(&lcd, 80,55,(char *)"C",0xf800,0,FONT_16x32,DRAW_TO_LCD | DRAW_TO_RAM);
    spilcdWriteString(&lcd, 96,55,(char *)"O",0x6e0,0,FONT_16x32,DRAW_TO_LCD | DRAW_TO_RAM);
    spilcdWriteString(&lcd, 112,55,(char *)"L",0x1f,0,FONT_16x32,DRAW_TO_LCD | DRAW_TO_RAM);
    spilcdWriteString(&lcd, 128,55,(char *)"O",0xf81f,0,FONT_16x32,DRAW_TO_LCD | DRAW_TO_RAM);
    spilcdWriteString(&lcd, 144,55,(char *)"R",0xffe0,0,FONT_16x32,DRAW_TO_LCD | DRAW_TO_RAM);
    i = 0;
    for (y=0; y<HEIGHT; y++)
    {
      for (x=0; x<WIDTH; x++)
      {
        color = pBitmap[x + (y*WIDTH)];
        if (color != 0) // pixel set?
        {
          grain[i].x = x*256; grain[i].y = y*256;
          grain[i].vx = grain[i].vy = 0; // Initial velocity is zero
          grain[i].color = color;
          i++;
          if (i == N_GRAINS) return;
        }
      } // for x
    } // for y
//    Serial.println(i, DEC);
    
  }
  
} /* ResetGrains() */

void SandDemo(void)
{
int16_t x, y, z;
int32_t v2; // Velocity squared
int16_t ax, ay, az;
//signed int        oldidx, newidx;
signed int        newx, newy;
signed int        x1, y1, x2, y2;
int i;
uint16_t *pBitmap = spilcdGetBuffer(&lcd);
uint16_t u16Flags[5]; // divide the display into 16x16 blocks for quicker refresh
int iFrame = 0;
uint32_t oldy, frac, ysum;

  ResetGrains(0);
//  delay(2000);
  memset(u16Flags,0,sizeof(u16Flags));
  while (1)
  {
    iFrame++;
    if ((iFrame & 7) == 0)
    {
      if (GetButtons())
       return;
    }
//    if (iFrame & 1) // update display if we didn't check the buttons
//    {
//      spilcdShowBuffer(&lcd, 0,0,WIDTH,HEIGHT, DRAW_TO_LCD);
//    }
//    if (bConnected) // use the left analog stick to simulate gravity
//    {
//      ax = gp.iLJoyX / 4;
//      ay = gp.iLJoyY / 4;
//    }
//    else // use the accelerometer
    {
      IMURead(&x, &y, &z);
      ax = -y / 512; // Transform accelerometer axes
      ay = -x / 512;      // to grain coordinate space
      az = abs(z) / 2048; // Random motion factor
      az = (az >= 3) ? 1 : 4 - az;      // Clip & invert
      ax -= az;                         // Subtract motion factor from X, Y
      ay -= az;
    }
  // Apply 2D accelerometer vector to grain velocities...
  //
  // Theory of operation:
  // if the 2D vector of the new velocity is too big (sqrt is > 256), this means it might jump
  // over pixels. We want to limit the velocity to 1 pixel as a maximum.
  // To avoid using floating point math (sqrt + 2 multiplies + 2 divides)
  // Instead of normalizing the velocity to keep the same direction, we can trim the new
  // velocity to 5/8 of it's value. This is a reasonable approximation since the maximum
  // velocity impulse from the accelerometer is +/-64 (16384 / 256) and it gets added every frame
  //
  spilcdSetPosition(&lcd, 0, 0, WIDTH, HEIGHT, DRAW_TO_LCD);
  oldy = 0xffff;
  ysum = 0;
  frac = (HEIGHT * 65536) / N_GRAINS; // 16-bit fraction of how many grains per line to draw
  // Update the position of each grain, one at a time, checking for
  // collisions and having them react.  This really seems like it shouldn't
  // work, as only one grain is considered at a time while the rest are
  // regarded as stationary.  Yet this naive algorithm, taking many not-
  // technically-quite-correct steps, and repeated quickly enough,
  // visually integrates into something that somewhat resembles physics.
  // (I'd initially tried implementing this as a bunch of concurrent and
  // "realistic" elastic collisions among circular grains, but the
  // calculations and volument of code quickly got out of hand for both
  // the tiny 8-bit AVR microcontroller and my tiny dinosaur brain.)
  //
  // (x,y) to bytes mapping:
  // The SSD1306 has 8 rows of 128 bytes with the LSB of each byte at the top
  // In other words, bytes are oriented vertically with bit 0 as the top pixel
  // Part of my optimizations were writing the pixels into memory the same way they'll be
  // written to the display. This means calculating an offset and bit to test/set each pixel
  //
  for(i=0; i<N_GRAINS; i++) {
    // Update the display 1 line at a time
    ysum += frac;
    if ((ysum >> 16) != oldy)
    {
      oldy = (ysum >> 16);
      spilcdWriteDataBlock(&lcd, (uint8_t *)&pBitmap[oldy*WIDTH], WIDTH*2, DRAW_TO_LCD | DRAW_WITH_DMA);
    }
    // Update the velocity of each grain
    grain[i].vx += ax;// + random(5); // Add a little random impulse to each grain
    grain[i].vy += ay;// + random(5);
    v2 = (int32_t)(grain[i].vx*grain[i].vx) + (int32_t)(grain[i].vy*grain[i].vy);
    if (v2 >= 400000) // too big, trim it
    {
      grain[i].vx = (grain[i].vx * 5)/8; // quick and dirty way to avoid doing a 'real' divide
      grain[i].vy = (grain[i].vy * 5)/8;
    }
    // Check for collisions with walls and other grains
    newx = grain[i].x + grain[i].vx; // New position in grain space
    newy = grain[i].y + grain[i].vy;
    if(newx > MAX_X) {               // If grain would go out of bounds
      newx         = MAX_X;          // keep it inside, and
      grain[i].vx /= -2;             // give a slight bounce off the wall
    } else if(newx < 0) {
      newx         = 0;
      grain[i].vx /= -2;
    }
    if(newy > MAX_Y) {
      newy         = MAX_Y;
      grain[i].vy /= -2;
    } else if(newy < 0) {
      newy         = 0;
      grain[i].vy /= -2;
    }

    x1 = grain[i].x / 256; y1 = grain[i].y / 256; // old position
    x2 = newx / 256; y2 = newy / 256;
    if((x1 != x2 || y1 != y2) && // If grain is moving to a new pixel...
        (pBitmap[x2 + (y2*WIDTH)] != 0)) {       // but if that pixel is already occupied...
        // Try skidding along just one axis of motion if possible (start w/faster axis)
        if(abs(grain[i].vx) > abs(grain[i].vy)) { // X axis is faster
          y2 = grain[i].y / 256;
          if(pBitmap[x2 + (y2*WIDTH)] == 0) { // That pixel's free!  Take it!  But...
            newy         = grain[i].y; // Cancel Y motion
            grain[i].vy = (grain[i].vy /-2) + random(8);         // and bounce Y velocity
          } else { // X pixel is taken, so try Y...
            y2 = newy / 256; x2 = grain[i].x / 256;
            if(pBitmap[x2 + (y2*WIDTH)] == 0) { // Pixel is free, take it, but first...
              newx         = grain[i].x; // Cancel X motion
              grain[i].vx = (grain[i].vx /-2) + random(8);         // and bounce X velocity
            } else { // Both spots are occupied
              newx         = grain[i].x; // Cancel X & Y motion
              newy         = grain[i].y;
              grain[i].vx = (grain[i].vx /-2) + random(8);         // Bounce X & Y velocity
              grain[i].vy = (grain[i].vy /-2) + random(8);
            }
          }
        } else { // Y axis is faster
          y2 = newy / 256; x2 = grain[i].x / 256;
          if(pBitmap[x2 + (y2*WIDTH)] == 0) { // Pixel's free!  Take it!  But...
            newx         = grain[i].x; // Cancel X motion
            grain[i].vx = (grain[i].vx /-2) + random(8);        // and bounce X velocity
          } else { // Y pixel is taken, so try X...
            y2 = grain[i].y / 256; x2 = newx / 256;
            if(pBitmap[x2 + (y2*WIDTH)] == 0) { // Pixel is free, take it, but first...
              newy         = grain[i].y; // Cancel Y motion
              grain[i].vy = (grain[i].vy /-2) + random(8);        // and bounce Y velocity
            } else { // Both spots are occupied
              newx         = grain[i].x; // Cancel X & Y motion
              newy         = grain[i].y;
              grain[i].vx = (grain[i].vx /-2) + random(8);         // Bounce X & Y velocity
              grain[i].vy = (grain[i].vy /-2) + random(8);
            }
          }
        }
    }
    grain[i].x  = newx; // Update grain position
    grain[i].y  = newy; // possibly only a fractional change
    y2 = newy / 256; x2 = newx / 256;
    if (x1 != x2 || y1 != y2)
    {
      pBitmap[x1 + (y1*WIDTH)] = 0; // erase old pixel
      pBitmap[x2 + (y2*WIDTH)] = grain[i].color;  // Set new pixel
    }
  } // for i
  // Draw the last line since the fraction won't reach it
  spilcdWriteDataBlock(&lcd, (uint8_t *)&pBitmap[(HEIGHT-1)*WIDTH], WIDTH*2, DRAW_TO_LCD | DRAW_WITH_DMA);
  } // while (1)
} /* IMUTest() */

void AxpBrightness(uint8_t brightness)
{
    if (brightness > 12)
    {
        brightness = 12;
    }
    uint8_t buf;
    buf = I2CReadByte(AXP_ADDR, 0x28 );
    buf = ((buf & 0x0f) | (brightness << 4));
    I2CRegisterWrite(AXP_ADDR, 0x28, buf);
}
void AxpPowerUp()
{
    // Set LDO2 & LDO3(TFT_LED & TFT) 3.0V
    I2CRegisterWrite(AXP_ADDR, 0x28, 0xcc);

    // Set ADC sample rate to 200hz
    I2CRegisterWrite(AXP_ADDR, 0x84, 0b11110010);
   
    // Set ADC to All Enable
    I2CRegisterWrite(AXP_ADDR, 0x82, 0xff);

    // Bat charge voltage to 4.2, Current 100MA
    I2CRegisterWrite(AXP_ADDR, 0x33, 0xc0);

    // Depending on configuration enable LDO2, LDO3, DCDC1, DCDC3.
    byte buf = (I2CReadByte(AXP_ADDR, 0x12) & 0xef) | 0x4D;
//    if(disableLDO3) buf &= ~(1<<3);
//    if(disableLDO2) buf &= ~(1<<2);
//    if(disableDCDC3) buf &= ~(1<<1);
//    if(disableDCDC1) buf &= ~(1<<0);
    I2CRegisterWrite(AXP_ADDR, 0x12, buf);
     // 128ms power on, 4s power off
    I2CRegisterWrite(AXP_ADDR, 0x36, 0x0C);

    if (1) //if(!disableRTC)
    {
        // Set RTC voltage to 3.3V
        I2CRegisterWrite(AXP_ADDR, 0x91, 0xF0);

        // Set GPIO0 to LDO
        I2CRegisterWrite(AXP_ADDR, 0x90, 0x02);
    }

    // Disable vbus hold limit
    I2CRegisterWrite(AXP_ADDR, 0x30, 0x80);

    // Set temperature protection
    I2CRegisterWrite(AXP_ADDR, 0x39, 0xfc);

    // Enable RTC BAT charge
//    Write1Byte(0x35, 0xa2 & (disableRTC ? 0x7F : 0xFF));
    I2CRegisterWrite(AXP_ADDR, 0x35, 0xa2);
     // Enable bat detection
    I2CRegisterWrite(AXP_ADDR, 0x32, 0x46);

    // Set Power off voltage 3.0v
    I2CRegisterWrite(AXP_ADDR, 0x31 , (I2CReadByte(AXP_ADDR, 0x31) & 0xf8) | (1 << 2));

} /* AxpPowerUp() */

void setup() {
  Wire1.begin(21, 22);
  Wire1.setClock(400000);
  AxpPowerUp();
  pinMode(BUTTON_A, INPUT_PULLUP);
  pinMode(BUTTON_B, INPUT_PULLUP);
  IMUInit();
  AxpBrightness(10); // turn on backlight (0-12)
//int spilcdInit(int iLCDType, int iFlags, int32_t iSPIFreq, int iCSPin, int iDCPin, int iResetPin, int iLEDPin, int iMISOPin, int iMOSIPin, int iCLKPin);
  spilcdSetTXBuffer(ucTXBuf, sizeof(ucTXBuf));
  spilcdInit(&lcd, LCD_ST7789_135, FLAGS_NONE, 40000000, TFT_CS, TFT_DC, TFT_RST, -1, -1, TFT_MOSI, TFT_CLK); // M5Stick-V pin numbering, 40Mhz
  spilcdAllocBackbuffer(&lcd);
  spilcdSetOrientation(&lcd, LCD_ORIENTATION_270);
  spilcdFill(&lcd, 0, DRAW_TO_LCD);
}

void loop() {
  SandDemo();
}

I need a help, can u explain more? I try to reprodece it now, but, fail.... How i can get a raw string of data? TKS