Arduino Powered 3D Snake

3D_Snake.ino

/*
3D Snake

Classic snake running on a 3x3x3 LED cube, 
controlled by a two axis joystick, mounted together with an accelerometer.

*****************************
Video of working circuit:
https://goo.gl/photos/teLkEi5afuj9UzuS9
*****************************

Created February 2017
By Chandler Childs

*/
void setup();
// Configure Accelerometer ------
#include<Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
struct ChipData {
  int16_t x;
  int16_t y;
  int16_t z;
};
ChipData chip;
void initChip(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
}
void readChip(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  chip.x=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  chip.y=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  chip.z=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
}
// Accelerometer Calculations ---
struct Directions {
  int x = 0;
  int y = 0;
  int z = 0;
};
Directions directions;
void updateDirections(){
  int threshold = 8000;
  if(chip.x > threshold){
    directions.x = -1;
  } else if(chip.x < -threshold) {
    directions.x = 1;
  } else {
    directions.x = 0;
  }
  if(chip.y > threshold){
    directions.y = 1;
  } else if(chip.y < -threshold) {
    directions.y = -1;
  } else {
    directions.y = 0;
  }
  if(chip.z > threshold){
    directions.z = 1;
  } else if(chip.z < -threshold) {
    directions.z = -1;
  } else {
    directions.z = 0;
  }
}
void printDirections(){
  Serial.print(directions.x); 
  Serial.print(directions.y);
  Serial.println(directions.z);
}
// Configure pins for cube  -----
const int DIM = 3;
int zx_pins[3][3] = {
  { 5, 6, 7},
  { 8, 9,10},
  {11,12,13}
};
int y_pins[3] = { 4, 3, 2};
void initCubePins(){
    for(int i = 0; i < DIM; i++){
    for(int j = 0; j < DIM; j++){
     pinMode(zx_pins[i][j], OUTPUT);
    }
    pinMode(y_pins[i], OUTPUT);
  }
  for(int i = 0; i < DIM; i++){
    for(int j = 0; j < DIM; j++){
      digitalWrite(zx_pins[i][j], LOW);
    }
    digitalWrite(y_pins[i], HIGH);
  }
}
// Matrix Display Operations ----
int rate = 350;
int matrix[DIM][DIM][DIM];
void zeroMatrix(){
  for(int z = 0; z < DIM; z++){
    for(int y = 0; y < DIM; y++){
      for(int x = 0; x < DIM; x++){
        matrix[z][y][x] = 0;
      }
    }
  }
}
void displayMatrix(int grid[3][3][3], int frameDelay){
  while(frameDelay--){
    for(int z = 0; z < DIM; z++){
      for(int y = 0; y < DIM; y++){
        // Turn on y row of pins (by grounding annode)
        digitalWrite(y_pins[y], LOW);
          // Turn ON x pins accross row at (y,z)
          for(int x = 0; x < DIM; x++){
            digitalWrite(zx_pins[z][x], grid[z][y][x]);
          }
          // Turn OFF x pins accross row at (y,z)
          for(int x = 0; x < DIM; x++){
            digitalWrite(zx_pins[z][x], LOW);
          }
        // Turn off y row of pins (by pulling up annode)
        digitalWrite(y_pins[y], HIGH);
      }
    }
  }
}
void displayMatrix(){
  displayMatrix(matrix, rate);
}
// Joystick ----------------------
struct Joystick {
  int x_raw;
  int y_raw;
  int x;
  int y;
  int deltaX;
  int deltaY;
  int deltaZ;
}; Joystick joy;
void readJoystick(){
  joy.x_raw = analogRead(A1);
  joy.y_raw = analogRead(A0);
  int base = 512;
  int thresh = 200;
  
  if(joy.x_raw > base + thresh){
    joy.x = -1;
  }else if(joy.x_raw < base - thresh){
    joy.x = 1;
  }else{
    joy.x = 0;
  }
  if(joy.y_raw > base + thresh){
    joy.y = 1;
  }else if(joy.y_raw < base - thresh){
    joy.y = -1;
  }else{
    joy.y = 0;
  }
  if(abs(joy.x_raw - base) > abs(joy.y_raw - base)){
    joy.y = 0;
  }else{
    joy.x = 0;
  }
  
}
void updateJoyDirections(){
  // Controller Flat
  if(directions.x == 0 && directions.y == 0 && directions.z == 1){
    //Serial.println("Flat");
    joy.deltaX = joy.x;
    joy.deltaY = joy.y;   
    joy.deltaZ = 0; 
  }
  // Controller Clockwise
  if(directions.x == 1 && directions.y == 0 && directions.z == 0){
    //Serial.println("Clockwise");
    
    joy.deltaX = 0;
    joy.deltaY = joy.y;
    joy.deltaZ = joy.x;
    
  }
  // Controller CounterClockwise
  if(directions.x == -1 && directions.y == 0 && directions.z == 0){
    //Serial.println("CounterClockwise");
    joy.deltaX = 0;
    joy.deltaY = joy.y;
    joy.deltaZ = -joy.x;
  }
  // Controller PitchBack
  if(directions.x == 0 && directions.y == 1 && directions.z == 0){
    //Serial.println("PitchBack");
    joy.deltaX = joy.x;
    joy.deltaY = 0;
    joy.deltaZ = joy.y;
  }
  // Controller PitchForward
  if(directions.x == 0 && directions.y == -1 && directions.z == 0){
    //Serial.println("PitchForward");
    joy.deltaX = joy.x;
    joy.deltaY = 0;
    joy.deltaZ = -joy.y;
  }
  // Controller Upsidedown
  if(directions.x == 0 && directions.y == 0 && directions.z == -1){
    //Serial.println("Upsidedown");
    joy.deltaX = -joy.x;
    joy.deltaY = joy.y;   
    joy.deltaZ = 0; 
  }
  
}
// Snake -------------------------
bool gameOver = false;
struct Coordinate{
  int x = 0;
  int y = 0;
  int z = 0;
};
bool overlap(struct Coordinate a, struct Coordinate b){
  if(a.x == b.x && a.y == b.y && a.z == b.z) return true;
  else return false;
}
Coordinate randomCoordinate(){
  randomSeed(millis() * (analogRead(A0) + 1) * (analogRead(A1) + 1));
  
  Coordinate point;
  point.x = random(3);
  point.y = random(3);
  point.z = random(3);
  return point;  
}
Coordinate newHead;
Coordinate snake[27];
int snakeLength;
bool overlapSnake(struct Coordinate point){
  for(int i = 0; i < snakeLength; i++){
    if(overlap(point, snake[i])) return true;
  }
  
  return false;
}
Coordinate apple;
void generateApple(){
  Coordinate newApple = randomCoordinate();
  while(overlapSnake(newApple) || overlap(newApple, apple)){
    newApple = randomCoordinate();
  }
  apple = newApple;
  matrix[apple.z][apple.y][apple.x] = 1;
}
void initSnake(){
  snakeLength = 1;
  
  snake[0].x = 0;
  snake[0].y = 0;
  snake[0].z = 0;
  for(int i = snakeLength - 1; i >= 0; i--){
    matrix[snake[i].z][snake[i].y][snake[i].x] = 1;
    displayMatrix();
  }
  generateApple();
  
}
bool findCollision(struct Coordinate head){
  if(head.x < 0) return true;
  if(head.x >= DIM) return true;
  if(head.y < 0) return true;
  if(head.y >= DIM) return true;
  if(head.z < 0) return true;
  if(head.z >= DIM) return true;
  if(overlapSnake(head)) return true;
  return false;
}
bool moved(){
  return !overlap(newHead, snake[0]);
}
void checkGameOver(){
  Coordinate space = snake[0];  
  space.x += 1;
  if(!findCollision(space)) return;
  space.x -= 2;
  if(!findCollision(space)) return;
  space.x += 1;
  space.y += 1;
  if(!findCollision(space)) return;
  space.y -= 2;
  if(!findCollision(space)) return;
  space.y += 1;
  space.z += 1;
  if(!findCollision(space)) return;
  space.z -= 2;
  if(!findCollision(space)) return;
  //space.z += 1;
  gameOver = true;
    
}
int blinkPos = 0;
void blinkSnake(){
  if(blinkPos < snakeLength){
    if(blinkPos >= 0){
      matrix[snake[blinkPos].z][snake[blinkPos].y][snake[blinkPos].x] = 0;
      displayMatrix(matrix, 200);
      matrix[snake[blinkPos].z][snake[blinkPos].y][snake[blinkPos].x] = 1;
    }
    blinkPos++;
      
  } else blinkPos = -6;
}
void updateSnake(){
  newHead.x = snake[0].x + joy.deltaX;
  newHead.y = snake[0].y + joy.deltaY;
  newHead.z = snake[0].z + joy.deltaZ;
  if(moved() && !findCollision(newHead)){
    if(overlap(newHead, apple)){
      snakeLength++;
      generateApple();
      blinkPos = 0;
    }
    
    matrix[newHead.z][newHead.y][newHead.x] = 1;
    matrix[snake[snakeLength - 1].z][snake[snakeLength - 1].y][snake[snakeLength - 1].x] = 0;
    for(int i = snakeLength - 1; i > 0; i--){
      snake[i] = snake[i-1];
    }
    snake[0] = newHead;
    checkGameOver();
  }
  blinkSnake();
}
void displayScore(){
  int blank[DIM][DIM][DIM] = {{{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0}}};
  blank[apple.z][apple.y][apple.x] = 1;
  for(int i = 0; i < 6; i++){
    displayMatrix(blank, 500);
    displayMatrix(matrix, 500);
  }
  
  initCubePins();
  //Score
  int tens = floor(snakeLength/10.0);
  int ones = snakeLength - tens * 10;
  int score[DIM][DIM][DIM] = {{{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0}},{{0,0,0},{0,0,0},{0,0,0}}};
  displayMatrix(score, 1000); 
    int z = 2;
    int y = 2;
    int x = 2;
  for(int i = 0; i < ones; i++){
    if(z < 0){
      z = 2;
      y--;
    }
    score[z][y][x] = 1;
    z--;
  }
  for(int i = 0; i < tens; i++){
    score[2 - i][2][0] = 1;
  }
  bool clearScreen = false;
  while(analogRead(A0) > 0){
    if(!clearScreen) displayMatrix(score, 400);
    if(analogRead(A0) > 1020) {
      clearScreen = true;
      initCubePins();
    }
  }
  setup();
  gameOver = false;
  
}
// ------------------------------
// Setup and Loop ---------------
// ------------------------------
void setup() {
  randomSeed(millis());
  initChip();
  initCubePins();
  zeroMatrix();
  initSnake();
  //Serial.begin(9600);
}
void loop() {
    while(!gameOver){
      readChip();
      updateDirections();
      
      readJoystick();
      updateJoyDirections();
  
      updateSnake();
      
      displayMatrix();
    }
    displayScore();
}