/ Hallowing_Googly_Eye / Hallowing_Googly_Eye.ino
Hallowing_Googly_Eye.ino
1 // SPDX-FileCopyrightText: 2018 Phillip Burgess for Adafruit Industries 2 // 3 // SPDX-License-Identifier: MIT 4 5 // "Googly eye" demo for Adafruit Hallowing. Uses accelerometer for 6 // motion plus DMA and related shenanigans for smooth animation. 7 8 #include <Adafruit_LIS3DH.h> 9 #include <Adafruit_GFX.h> 10 #include <Adafruit_ST7735.h> 11 #include <Adafruit_ZeroDMA.h> 12 #include "graphics.h" 13 //#include "gritty.h" 14 15 #define G_SCALE 40.0 // Accel scale; no science, just looks good 16 #define ELASTICITY 0.80 // Edge-bounce coefficient (MUST be <1.0!) 17 #define DRAG 0.996 // Dampens motion slightly 18 19 #define PUPIL_RADIUS (PUPIL_SIZE / 2.0) // Radius of pupil, same units 20 // Pupil motion is computed as a single point constrained within a circle 21 // whose radius is the eye radius minus the pupil radius. 22 #define INNER_RADIUS (EYE_RADIUS - PUPIL_RADIUS) 23 24 #if defined(ADAFRUIT_HALLOWING) 25 #define TFT_CS 39 // Hallowing display control pins: chip select 26 #define TFT_RST 37 // Display reset 27 #define TFT_DC 38 // Display data/command select 28 #define TFT_BACKLIGHT 7 // Display backlight pin 29 #define TFT_SPI SPI 30 #define TFT_PERIPH PERIPH_SPI 31 Adafruit_LIS3DH accel = Adafruit_LIS3DH(); 32 #elif defined(ARDUINO_SAMD_CIRCUITPLAYGROUND_EXPRESS) 33 #define TFT_CS A7 // Display select 34 #define TFT_DC A6 // Display data/command pin 35 #define TFT_RST -1 // Display reset pin 36 #define TFT_BACKLIGHT A3 37 #define TFT_PERIPH PERIPH_SPI1 38 #define TFT_SPI SPI1 39 Adafruit_LIS3DH accel(&Wire1); 40 #endif 41 42 // For the sake of math comprehension and simplicity, movement takes place 43 // in a traditional Cartesian coordinate system (+Y is up), floating point 44 // values, grid units equal to pixels, with (0.0, 0.0) at the literal center 45 // of the screen (e.g. between pixels 63 and 64). Things get flipped to 46 // +Y down integer pixel units just before drawing. 47 float x = 0.0, y = 0.0, // Pupil position, start at center 48 vx = 0.0, vy = 0.0; // Pupil velocity (X,Y components) 49 uint32_t lastTime; // Last-rendered frame time, microseconds 50 bool firstFrame = true; // Force full-screen update on initial frame 51 52 // Declarations for various Hallowing hardware -- display, accelerometer 53 // and SPI rate & mode. 54 Adafruit_ST7735 tft = Adafruit_ST7735(&TFT_SPI, TFT_CS, TFT_DC, TFT_RST); 55 56 SPISettings settings(12000000, MSBFIRST, SPI_MODE0); 57 58 // Declarations related to DMA (direct memory access), which lets us walk 59 // and chew gum at the same time. This is VERY specific to SAMD chips and 60 // means this is not trivially ported to other devices. 61 Adafruit_ZeroDMA dma; 62 DmacDescriptor *descriptor; 63 uint16_t dmaBuf[2][128]; 64 uint8_t dmaIdx = 0; // Active DMA buffer # (alternate fill/send) 65 66 // DMA transfer-in-progress indicator and callback 67 static volatile bool dma_busy = false; 68 static void dma_callback(Adafruit_ZeroDMA *dma) { 69 dma_busy = false; 70 } 71 72 // SETUP FUNCTION -- runs once at startup ---------------------------------- 73 74 void setup(void) { 75 // Hardware init 76 tft.initR(INITR_144GREENTAB); 77 tft.setRotation(2); // Display is rotated 180° on Hallowing 78 tft.fillScreen(0); 79 80 pinMode(TFT_BACKLIGHT, OUTPUT); 81 82 if(accel.begin(0x18) || accel.begin(0x19)) { 83 accel.setRange(LIS3DH_RANGE_8_G); 84 } 85 86 // Set up SPI DMA. While the Hallowing has a known SPI peripheral and 87 // this could be much simpler, the extra code here will help if adapting 88 // the sketch to other SAMD boards (Feather M0, M4, etc.) 89 int dmac_id; 90 volatile uint32_t *data_reg; 91 dma.allocate(); 92 if(&TFT_PERIPH == &sercom0) { 93 dma.setTrigger(SERCOM0_DMAC_ID_TX); 94 data_reg = &SERCOM0->SPI.DATA.reg; 95 #if defined SERCOM1 96 } else if(&TFT_PERIPH == &sercom1) { 97 dma.setTrigger(SERCOM1_DMAC_ID_TX); 98 data_reg = &SERCOM1->SPI.DATA.reg; 99 #endif 100 #if defined SERCOM2 101 } else if(&TFT_PERIPH == &sercom2) { 102 dma.setTrigger(SERCOM2_DMAC_ID_TX); 103 data_reg = &SERCOM2->SPI.DATA.reg; 104 #endif 105 #if defined SERCOM3 106 } else if(&TFT_PERIPH == &sercom3) { 107 dma.setTrigger(SERCOM3_DMAC_ID_TX); 108 data_reg = &SERCOM3->SPI.DATA.reg; 109 #endif 110 #if defined SERCOM4 111 } else if(&TFT_PERIPH == &sercom4) { 112 dma.setTrigger(SERCOM4_DMAC_ID_TX); 113 data_reg = &SERCOM4->SPI.DATA.reg; 114 #endif 115 #if defined SERCOM5 116 } else if(&TFT_PERIPH == &sercom5) { 117 dma.setTrigger(SERCOM5_DMAC_ID_TX); 118 data_reg = &SERCOM5->SPI.DATA.reg; 119 #endif 120 } 121 dma.setAction(DMA_TRIGGER_ACTON_BEAT); 122 descriptor = dma.addDescriptor( 123 NULL, // move data 124 (void *)data_reg, // to here 125 sizeof dmaBuf[0], // this many... 126 DMA_BEAT_SIZE_BYTE, // bytes/hword/words 127 true, // increment source addr? 128 false); // increment dest addr? 129 dma.setCallback(dma_callback); 130 131 digitalWrite(TFT_BACKLIGHT, HIGH); 132 lastTime = micros(); 133 } 134 135 // LOOP FUNCTION -- repeats indefinitely ----------------------------------- 136 137 void loop(void) { 138 accel.read(); 139 140 // Get time since last frame, in floating-point seconds 141 uint32_t t = micros(); 142 float elapsed = (float)(t - lastTime) / 1000000.0; 143 lastTime = t; 144 145 // Scale accelerometer readings based on an empirically-derived constant 146 // (i.e. looks good, nothing scientific) and time since prior frame. 147 // On HalloWing, accelerometer's Y axis is horizontal, X axis is vertical, 148 // (vs screen's and conventional Cartesian coords being X horizontal, 149 // Y vertical), so swap while we're here, store in ax, ay; 150 float scale = G_SCALE * elapsed; 151 float ax = accel.y_g * scale, // Horizontal acceleration, pixel units 152 ay = accel.x_g * scale; // Vertical acceleration " 153 154 #if defined(ARDUINO_SAMD_CIRCUITPLAYGROUND_EXPRESS) 155 // CPX has different accel orientations 156 float temp = ay; 157 ay = ax; 158 ax = -temp; 159 #endif 160 161 // Add scaled accelerometer readings to pupil velocity, store interim 162 // values in vxNew, vyNew...a little friction prevents infinite bounce. 163 float vxNew = (vx + ax) * DRAG, 164 vyNew = (vy + ay) * DRAG; 165 166 // Limit velocity to pupil size to avoid certain overshoot situations 167 float v = vxNew * vxNew + vyNew * vyNew; 168 if(v > (PUPIL_SIZE * PUPIL_SIZE)) { 169 v = PUPIL_SIZE / sqrt(v); 170 vxNew *= v; 171 vyNew *= v; 172 } 173 174 // Add new velocity to prior position, store interim in xNew, yNew; 175 float xNew = x + vxNew, 176 yNew = y + vyNew; 177 178 // Get pupil position (center point) distance-squared from origin... 179 // here's why we put (0,0) at the center... 180 float d = xNew * xNew + yNew * yNew; 181 182 // Is pupil heading out of the eye constraints? No need for a sqrt() 183 // yet...since we're just comparing against a constant at this point, 184 // we can square the constant instead, avoid math... 185 float r2 = INNER_RADIUS * INNER_RADIUS; // r^2 186 if(d >= r2) { 187 188 // New pupil center position is outside the circle, now the math 189 // suddenly gets intense... 190 191 float dx = xNew - x, // Vector from old to new position 192 dy = yNew - y; // (crosses INNER_RADIUS perimeter) 193 194 // Find intersections between unbounded line and circle... 195 float x2 = x * x, // x^2 196 y2 = y * y, // y^2 197 a2 = dx * dx, // dx^2 198 b2 = dy * dy, // dy^2 199 a2b2 = a2 + b2, 200 n1, n2, 201 n = a2*r2 - a2*y2 + 2.0*dx*dy*x*y + b2*r2 - b2*x2; 202 if((n >= 0.0) & (a2b2 > 0.0)) { 203 // Because there's a square root here... 204 n = sqrt(n); 205 // There's two possible intersection points. Consider both... 206 n1 = (n - dx * x - dy * y) / a2b2; 207 n2 = -(n + dx * x + dy * y) / a2b2; 208 } else { 209 n1 = n2 = 0.0; // Avoid divide-by-zero 210 } 211 // ...and use the 'larger' one (may be -0.0, that's OK!) 212 if(n2 > n1) n1 = n2; 213 float ix = x + dx * n1, // Single intersection point of 214 iy = y + dy * n1; // movement vector and circle. 215 216 // Pupil needs to be constrained within eye circle, but we can't just 217 // stop it's motion at the edge, that's cheesy and looks wrong. On its 218 // way out, it was moving with a certain direction and speed, and needs 219 // to bounce back in with suitable changes to both... 220 221 float mag1 = sqrt(dx * dx + dy * dy), // Full velocity vector magnitude 222 dx1 = (ix - x), // Vector from prior pupil pos. 223 dy1 = (iy - y), // to point of edge intersection 224 mag2 = sqrt(dx1*dx1 + dy1*dy1); // Magnitude of above vector 225 // Difference between the above two magnitudes is the distance the pupil 226 // will bounce back into the eye circle on this frame (i.e. it rarely 227 // stops exactly at the edge...in the course of a single frame, it will 228 // be moving outward a certain amount, contact edge, and move inward 229 // a certain amount. The latter amount is scaled back slightly as it 230 // loses some energy in edge the collision. 231 float mag3 = (mag1 - mag2) * ELASTICITY; 232 233 float ax = -ix / INNER_RADIUS, // Unit surface normal (magnitude 1.0) 234 ay = -iy / INNER_RADIUS, // at contact point with circle. 235 rx, ry; // Reverse velocity vector, normalized 236 if(mag1 > 0.0) { 237 rx = -dx / mag1; 238 ry = -dy / mag1; 239 } else { 240 rx = ry = 0.0; 241 } 242 // Dot product between the two vectors is cosine of angle between them 243 float dot = rx * ax + ry * ay, 244 rpx = ax * dot, // Point to reflect across 245 rpy = ay * dot; 246 rx += (rpx - rx) * 2.0; // Reflect velocity vector across point 247 ry += (rpy - ry) * 2.0; // (still normalized) 248 249 // New position is the intersection point plus the reflected vector 250 // scaled by mag3 (the elasticity-reduced velocity remainder). 251 xNew = ix + rx * mag3; 252 yNew = iy + ry * mag3; 253 254 // Velocity magnitude is scaled by the elasticity coefficient. 255 mag1 *= ELASTICITY; 256 vxNew = rx * mag1; 257 vyNew = ry * mag1; 258 } 259 260 int x1, y1, x2, y2, // Bounding rect of screen update area 261 px1 = 64 + (int)xNew - PUPIL_SIZE / 2, // Bounding rect of new pupil pos. only 262 px2 = 64 + (int)xNew + PUPIL_SIZE / 2 - 1, 263 py1 = 64 - (int)yNew - PUPIL_SIZE / 2, 264 py2 = 64 - (int)yNew + PUPIL_SIZE / 2 - 1; 265 266 if(firstFrame) { 267 x1 = y1 = 0; 268 x2 = y2 = 127; 269 firstFrame = false; 270 } else { 271 if(xNew >= x) { // Moving right 272 x1 = 64 + (int)x - PUPIL_SIZE / 2; 273 x2 = 64 + (int)xNew + PUPIL_SIZE / 2 - 1; 274 } else { // Moving left 275 x1 = 64 + (int)xNew - PUPIL_SIZE / 2; 276 x2 = 64 + (int)x + PUPIL_SIZE / 2 - 1; 277 } 278 if(yNew >= y) { // Moving up (still using +Y Cartesian coords) 279 y1 = 64 - (int)yNew - PUPIL_SIZE / 2; 280 y2 = 64 - (int)y + PUPIL_SIZE / 2 - 1; 281 } else { // Moving down 282 y1 = 64 - (int)y - PUPIL_SIZE / 2; 283 y2 = 64 - (int)yNew + PUPIL_SIZE / 2 - 1; 284 } 285 } 286 287 x = xNew; // Save new position, velocity 288 y = yNew; 289 vx = vxNew; 290 vy = vyNew; 291 292 // Clip update rect. This shouldn't be necessary, but it looks 293 // like very occasionally an off-limits situation may occur, so... 294 if(x1 < 0) x1 = 0; 295 if(y1 < 0) y1 = 0; 296 if(x2 > 127) x2 = 127; 297 if(y2 > 127) y2 = 127; 298 299 TFT_SPI.beginTransaction(settings); // SPI init 300 digitalWrite(TFT_CS, LOW); // Chip select 301 tft.setAddrWindow(x1, y1, x2-x1+1, y2-y1+1); 302 digitalWrite(TFT_CS, LOW); // Re-select after addr function 303 digitalWrite(TFT_DC, HIGH); // Data mode... 304 305 uint16_t *dmaPtr; // Pointer into DMA output buffer (16 bits/pixel) 306 uint8_t col, row; // X,Y pixel counters 307 uint16_t result, // Expanded 16-bit pixel color 308 nBytes; // Size of DMA transfer 309 310 descriptor->BTCNT.reg = nBytes = (x2 - x1 + 1) * 2; 311 312 #ifdef COLOR_EYE 313 uint16_t *srcPtr1, // Pointer into eye background bitmap (16bpp) 314 *srcPtr2, // Pointer into pupil bitmap (16bpp) 315 rgb1, rgb2; // Colors of above 316 uint8_t red1, green1, blue1, // Color components 317 red2, green2, blue2; 318 319 // Process rows ABOVE pupil 320 for(row=y1; row<py1; row++) { 321 dmaPtr = &dmaBuf[dmaIdx][0]; 322 srcPtr1 = (uint16_t *)&borderData[row][x1]; 323 for(col=x1; col<=x2; col++) { 324 *dmaPtr++ = __builtin_bswap16(*srcPtr1++); 325 } 326 dmaXfer(nBytes); 327 } 328 329 // Process rows WITH pupil 330 for(; row<=py2; row++) { 331 dmaPtr = &dmaBuf[dmaIdx][0]; // Output to start of DMA buf 332 srcPtr1 = (uint16_t *)&borderData[row][x1]; // Initial byte of eye border 333 srcPtr2 = (uint16_t *)&pupilData[row-py1][0]; // Initial byte of pupil 334 for(col=x1; col<px1; col++) { // LEFT of pupil 335 *dmaPtr++ = __builtin_bswap16(*srcPtr1++); 336 } 337 for(; col<=px2; col++) { // Overlap pupil 338 rgb1 = *srcPtr1++; 339 rgb2 = *srcPtr2++; 340 red1 = rgb1 >> 11; // 5 bits red 341 green1 = (rgb1 >> 5) & 0x3F; // 6 bits green 342 blue1 = rgb1 & 0x1F; // 5 bits blue 343 red2 = rgb2 >> 11; 344 green2 = (rgb2 >> 5) & 0x3F; 345 blue2 = rgb2 & 0x1F; 346 red1 = (red1 * (red2 + 1)) / 32; // Multiply each 347 green1 = (green1 * (green2 + 1)) / 64; 348 blue1 = (blue1 * (blue2 + 1)) / 32; 349 rgb1 = ((uint16_t)red1 << 11) | ((uint16_t)green1 << 5) | blue1; 350 *dmaPtr++ = __builtin_bswap16(rgb1); 351 } 352 for(; col<=x2; col++) { // RIGHT of pupil 353 *dmaPtr++ = __builtin_bswap16(*srcPtr1++); 354 } 355 dmaXfer(nBytes); 356 } 357 358 // Process rows BELOW pupil 359 for(; row<=y2; row++) { 360 dmaPtr = &dmaBuf[dmaIdx][0]; 361 srcPtr1 = (uint16_t *)&borderData[row][x1]; 362 for(col=x1; col<=x2; col++) { 363 *dmaPtr++ = __builtin_bswap16(*srcPtr1++); 364 } 365 dmaXfer(nBytes); 366 } 367 368 #else // Grayscale eye 369 370 uint8_t *srcPtr1, // Pointer into eye background bitmap (8bpp) 371 *srcPtr2, // Pointer into pupil bitmap (8bpp) 372 b; // Resulting pixel brightness (0-255) 373 374 // Macro converts 8-bit grayscale to 16-bit '565' RGB value 375 #define STORE565(x) \ 376 result = (((x * 0x801) >> 3) & 0xF81F) | ((x & 0xFC) << 3); \ 377 *dmaPtr++ = __builtin_bswap16(result); 378 379 // Process rows ABOVE pupil 380 for(row=y1; row<py1; row++) { 381 dmaPtr = &dmaBuf[dmaIdx][0]; 382 srcPtr1 = (uint8_t *)&borderData[row][x1]; 383 for(col=x1; col<=x2; col++) { 384 b = *srcPtr1++; 385 STORE565(b) 386 } 387 dmaXfer(nBytes); 388 } 389 390 // Process rows WITH pupil 391 for(; row<=py2; row++) { 392 dmaPtr = &dmaBuf[dmaIdx][0]; // Output to start of DMA buf 393 srcPtr1 = (uint8_t *)&borderData[row][x1]; // Initial byte of eye border 394 srcPtr2 = (uint8_t *)&pupilData[row-py1][0]; // Initial byte of pupil 395 for(col=x1; col<px1; col++) { // LEFT of pupil 396 b = *srcPtr1++; 397 STORE565(b) 398 } 399 for(; col<=px2; col++) { // Overlap pupil 400 b = (*srcPtr1++ * (*srcPtr2++ + 1)) >> 8; 401 STORE565(b) 402 } 403 for(; col<=x2; col++) { // RIGHT of pupil 404 b = *srcPtr1++; 405 STORE565(b) 406 } 407 dmaXfer(nBytes); 408 } 409 410 // Process rows BELOW pupil 411 for(; row<=y2; row++) { 412 dmaPtr = &dmaBuf[dmaIdx][0]; 413 srcPtr1 = (uint8_t *)&borderData[row][x1]; 414 for(col=x1; col<=x2; col++) { 415 b = *srcPtr1++; 416 STORE565(b) 417 } 418 dmaXfer(nBytes); 419 } 420 421 #endif // !COLOR_EYE 422 423 while(dma_busy); // Wait for last DMA transfer to complete 424 digitalWrite(TFT_CS, HIGH); // Deselect 425 TFT_SPI.endTransaction(); // SPI done 426 } 427 428 void dmaXfer(uint16_t n) { // n = Transfer size in bytes 429 while(dma_busy); // Wait for prior DMA transfer to finish 430 // Set up DMA transfer from newly-filled buffer 431 descriptor->SRCADDR.reg = (uint32_t)&dmaBuf[dmaIdx] + n; 432 dma_busy = true; // Flag as busy 433 dma.startJob(); // Start new DMA transfer 434 dmaIdx = 1 - dmaIdx; // And swap DMA buffer indices 435 }