Gyro Pong game box

/* Arduino UNO with MPU-6050 over I2C (GY-521 module)
 *  Tom Tobback Nov 2016 - BuffaloLabs
 *  built in box, Atmega328 16MHz on perf board, Duemilanove board type
 *
 *  PONG GAME:
 *  2 players, 5 points
 *
 *  GYRO GAME:
 *  -tilt the sensor to move the ball around
 *  -catch the squares
 *  -do not touch the borders
 *  -catch as many as you can in 1min
 *
 *  GY-521 module
 *  5V
 *  GND
 *  SDA=A4
 *  SCL=A5
 *
 *  1.8'' TFT LCD
 *   LED 220ohm resistor to 5V
 *   SCK D13
 *   SDA D11
 *   A0/DC D9
 *   RESET D8
 *   CS D10
 *   GND
 *   Vcc 5V
 *
 *  gyro/pong switch on D7
 *  potentiometers on A0 and A1
 *  buzzer on D3
 *  LED on D2
 *  high score in EEPROM
 *  button on RST
 *
 *  IMU code:
 *  Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").
 Contact information
 -------------------
 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <EEPROM.h>

#include <TFT.h>  // Arduino LCD library
#include <SPI.h>

#define cs   10
#define dc   9
#define rst  8

TFT TFTscreen = TFT(cs, dc, rst);

int tftWidth;
int tftHeight;

////////////////////// GYRO ////////////////////////////////////////////////////

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

float roll;
float pitch;
float posX;
float posY;
int targetX;
int targetY;
int success_counter = 0;
long start_time;
boolean gyro;         // switch gyro/pong at startup
int sec = 60;
int high_score;
int time_left;

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

/////////////////////////////// PONG //////////////////////////////////////////////

const float pi = 3.14159265;
int player1 = -1;
int player2 = -1;
int player1_old;
int player2_old;
float ball_x = 64;
float ball_y = 32;
float ball_x_old;
float ball_y_old;
float ball_angle;  // in radians 0 -> 2pi
int speed = 2;      // normal = 2
int counter = 0;
int score1 = 0;
int score2 = 0;

long time;
long interval = 5000;

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void setup() {

  if (EEPROM.read(0) == 255) {
    EEPROM.write(0, 0);            // reset GYRO high score to 0
  }

  TFTscreen.begin();
  tftWidth = TFTscreen.width();
  tftHeight = TFTscreen.height();
  TFTscreen.background(0, 0, 0);

  pinMode(2, OUTPUT);             // LED
  pinMode(3, OUTPUT);             // buzzer
  pinMode(6, INPUT_PULLUP);       // button restart

  digitalWrite(2, HIGH);
  tone(3, 500);
  delay(100);
  tone(3, 1000);
  delay(100);
  noTone(3);
  digitalWrite(2, LOW);

  pinMode(7, INPUT_PULLUP);       // gyro or pong
  gyro = digitalRead(7);

  Serial.begin(115200);
  Serial.println(tftWidth);
  Serial.println(tftHeight);
  if (gyro) {
    initGyro();
    high_score = EEPROM.read(0);
    time_left = sec;
    randomSeed(analogRead(0));
    setRandom();
    drawIntroGyro();
  }
  else {
    drawIntroPong();
    ball_angle = float(random(100, 200)) / 100;
  }

  delay(2000);
  TFTscreen.background(0, 0, 0);                      // black    -- clear screen

  if (gyro) {
    drawGyroStart();                                // box
    drawGyroScore(0);
    posX = tftWidth / 2;
    posY = tftHeight / 2;
  }
  else {
    drawPongScores(0, 0);                            // PONG initial scores
  }

  timer = micros();
  start_time = millis();
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void loop() {

  if (gyro) {
    getGyro();      // this takes about 2msec
    pitch = - pitch;  // depending on orientation of sensor
    roll = - roll;
    drawGyro();
  }
  else {
    player1_old = player1;
    player2_old = player2;
    player1 = constrain(map(analogRead(A0), 200, 500, tftHeight - 25, 0), 0, tftHeight - 25 ); // potentiometer on pin A0
    player2 = constrain(map(analogRead(A1), 200, 500, tftHeight - 25, 0), 0, tftHeight - 25 ); // potentiometer on pin A1
    drawPong();
  }
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////  GYRO  /////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void initGyro() {
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll  = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////

void getGyro() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = (i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = (i2cData[12] << 8) | i2cData[13];

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll  = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate() * dt;

  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;

  /* Print Data */
#if 0 // Set to 1 to activate
  Serial.print(accX); Serial.print("\t");
  Serial.print(accY); Serial.print("\t");
  Serial.print(accZ); Serial.print("\t");

  Serial.print(gyroX); Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ); Serial.print("\t");

  Serial.print("\t");

  Serial.print(roll); Serial.print("\t");
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  Serial.print(pitch); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  Serial.print(kalAngleY); Serial.print("\t");

  Serial.print("\t");

  double temperature = (double)tempRaw / 340.0 + 36.53;
  Serial.print(temperature); Serial.print("\t");
  Serial.println();
#endif
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawGyro() {

  TFTscreen.noStroke();

  // erase ball
  TFTscreen.fill(0, 0, 0);                          // black
  TFTscreen.circle(posX, posY, 3);

  posX -= roll / 10.0;
  posY += pitch / 10.0;
  posX = constrain(posX, 32, tftWidth);
  posY = constrain(posY, 1, tftHeight);

  // draw ball
  TFTscreen.fill(0, 255, 0);                          // green
  TFTscreen.circle(posX, posY, 3);

  int current_time_left = abs(sec - (millis() - start_time) / 1000.0);
  if (current_time_left < time_left) {
    // erase time
    char timer[4];
    String str_timer = String(time_left);
    str_timer.toCharArray(timer, 4);
    TFTscreen.stroke(0, 0, 0);                    // black
    TFTscreen.text(timer, 5, 100);
    // draw time
    str_timer = String(current_time_left);
    str_timer.toCharArray(timer, 4);
    TFTscreen.stroke(255, 255, 255);                    // white
    TFTscreen.text(timer, 5, 100);
    time_left = current_time_left;
  }

  if (posX > (targetX - 4) && posX < (targetX + 4) && posY > (targetY - 4) && posY < (targetY + 4)) {   // CATCH
    success_counter++;
    digitalWrite(2, HIGH);
    drawGyroScore(success_counter);
    TFTscreen.noStroke();
    // erase target
    TFTscreen.fill(0, 0, 0);                          // black
    TFTscreen.rect(targetX - 3, targetY - 3, 6, 6);     // target
    setRandom();
    // draw target
    TFTscreen.fill(0, 0, 255);                          // red
    TFTscreen.rect(targetX - 3, targetY - 3, 6, 6);     // target
    tone(3, 1000, 100);
  }
  else {
    digitalWrite(2, LOW);
  }

  if (posX < 36 || posX > tftWidth - 5 || posY < 5 || posY > tftHeight - 5) {                         // HIT BORDER = GAME OVER
    tone(3, 100, 1000);
    TFTscreen.stroke(0, 0, 255);                    // red
    TFTscreen.text("GAME OVER", 40, 40);
    TFTscreen.stroke(255, 0, 0);                    // blue
    TFTscreen.setTextSize(1);
    TFTscreen.text("press button", 60, 70);
    TFTscreen.text("to start again", 55, 90);
    while (1) {                                                               // while wait for reset, short blink
      digitalWrite(2, HIGH);
      delay(20);
      digitalWrite(2, LOW);
      delay(2000);
    }
  }

  if ((millis() - start_time) / 1000.0 >= sec) {                                        // many SEC = GAME OVER
    if (success_counter > high_score) {                                               // new high score
      high_score = success_counter;
      EEPROM.write(0, high_score);

      TFTscreen.background(0, 255, 0);            // green
      TFTscreen.stroke(0, 0, 255);                // red
      TFTscreen.text("NEW", 70, 20);
      TFTscreen.text("HIGH SCORE!", 20, 40);
      char score[4];
      String str_score = String(high_score);
      str_score.toCharArray(score, 4);
      TFTscreen.text(score, 75, 60);
      TFTscreen.setTextSize(1);
      TFTscreen.text("congratulations!", 30, 90);
      TFTscreen.text("press button to start", 20, 100);

      digitalWrite(2, HIGH);
      for (int i = 1000; i < 4000; i += 5) {
        tone(3, i);
        delay(1);
      }
      for (int i = 4000; i > 1000; i -= 5) {
        tone(3, i);
        delay(1);
      }
      noTone(3);
      digitalWrite(2, LOW);
    }
    else {                                                                            // no new high score
      TFTscreen.stroke(0, 255, 0);                    // green
      TFTscreen.text("GAME OVER", 40, 40);
      TFTscreen.stroke(255, 0, 0);                    // blue
      TFTscreen.setTextSize(1);
      TFTscreen.text("press button", 60, 70);
      TFTscreen.text("to start again", 55, 90);
      for (int i = 0; i < 30; i++) {
        tone(3, 2000);
        digitalWrite(2, HIGH);
        delay(20);
        noTone(3);
        digitalWrite(2, LOW);
        delay(20);
      }
    }
    while (1) {                                                               // while wait for reset, short blink
      digitalWrite(2, HIGH);
      delay(20);
      digitalWrite(2, LOW);
      delay(2000);
    }
  }
  delay(10);                     // speed of GYRO game
}

void setRandom() {
  targetX = random(36, tftWidth - 4);  // border is 67 and 126
  targetY = random(4, tftHeight - 4);    // border is 1 and 62
}


////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawIntroGyro() {
  TFTscreen.background(0, 0, 0);  // black
  TFTscreen.stroke(255, 255, 255); // white
  TFTscreen.setTextSize(2);
  TFTscreen.text("BuffaloLabs", 18, 20);
  TFTscreen.text("GYRO GAME", 30, 50);
  TFTscreen.text("highscore:", 10, 80);
  char highscore[4];
  String str_high = String(high_score);
  str_high.toCharArray(highscore, 4);
  TFTscreen.text(highscore, 130, 80);
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawGyroStart() {
  TFTscreen.background(0, 0, 0);  // black
  TFTscreen.stroke(255, 255, 255); // white
  TFTscreen.rect(32, 0, tftWidth - 32, tftHeight);

  TFTscreen.noStroke();
  // draw target
  TFTscreen.fill(0, 0, 255);                          // red
  TFTscreen.rect(targetX - 3, targetY - 3, 6, 6);     // target

}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawGyroScore(int s) {
  char score[4];
  String str_score = String(s - 1);
  str_score.toCharArray(score, 4);
  TFTscreen.stroke(0, 0, 0);                    // black
  TFTscreen.text(score, 5, 20);
  str_score = String(s);
  str_score.toCharArray(score, 4);
  TFTscreen.stroke(255, 255, 255);                    // white
  TFTscreen.text(score, 5, 20);

}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////  PONG  //////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawIntroPong() {
  TFTscreen.background(0, 0, 0);  // black
  TFTscreen.stroke(255, 255, 255); // white
  TFTscreen.setTextSize(2);
  TFTscreen.text("BuffaloLabs", 18, 20);
  TFTscreen.text("PONG GAME", 30, 50);
  TFTscreen.text("2 players ", 30, 80);
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawPongScores(int s1, int s2) {

  TFTscreen.setTextSize(2);
  TFTscreen.stroke(0, 0, 0);                    // black
  char score[4];
  String str_score = String(s1 - 1);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 20, 100);
  str_score = String(s2 - 1);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 130, 100);

  TFTscreen.stroke(255, 255, 255);                    // white
  str_score = String(s1);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 20, 100);
  TFTscreen.stroke(255, 255, 255);
  str_score = String(s2);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 130, 100);

  delay(1000);
  TFTscreen.stroke(0, 0, 0);                    // black
  str_score = String(s1);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 20, 100);
  str_score = String(s2);
  str_score.toCharArray(score, 4);
  TFTscreen.text(score, 130, 100);

  TFTscreen.noStroke();               // better speed without stroke

}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawPong() {

  unsigned long timestamp = millis();
  // erase ball
  TFTscreen.fill(0, 0, 0);                          // black
  TFTscreen.circle(ball_x + 0.5, ball_y + 0.5, 3);      // for rounding instead of truncating

  Serial.println(ball_angle);
  ball_y += speed * cos (ball_angle);
  ball_x += speed * sin (ball_angle);

  // draw ball
  TFTscreen.fill(255, 255, 255);                          // white
  TFTscreen.circle(ball_x + 0.5, ball_y + 0.5, 2);      // for rounding instead of truncating

  float noise = float(random(50)) / 100;

  // bounce on sides
  if (ball_y >= (tftHeight - 3) || ball_y <= 2) ball_angle = pi - ball_angle;
  checkBallAngle();

  if (ball_x <= 9) {                                                  // bounce by player1
    if  (ball_y < (player1 + 25) && ball_y > player1) {
      digitalWrite(2, HIGH);
      ball_angle = - ball_angle + noise;
      checkBallAngle();
      tone(3, 100, 100);
      counter += 1;
      ball_x++;
      digitalWrite(2, LOW);
    }
    else
    { // player2 wins
      for (int i = 1000; i > 500; i += -1) {
        tone(3, i);
        delay(1);
      }
      noTone(3);
      score2++;
      drawPongScores(score1, score2);
      if (score1 + score2 >= 5) drawPongEnd();
      // erase ball
      TFTscreen.fill(0, 0, 0);                          // black
      TFTscreen.circle(ball_x + 0.5, ball_y + 0.5, 3);      // for rounding instead of truncating
      delay(1000);
      ball_x = 20;
      ball_y = 64;
      ball_angle = 1.5;
      counter = 5;
    }
  }

  if (ball_x >= (tftWidth - 11)) {                                        // bounce by player2
    if  (ball_y < (player2 + 25) && ball_y > player2) {
      digitalWrite(2, HIGH);
      ball_angle = - ball_angle + noise;
      checkBallAngle();
      tone(3, 100, 100);
      counter += 1;
      ball_x--;
      digitalWrite(2, LOW);
    }
    else
    { // player1 wins
      for (int i = 1000; i > 500; i += -1) {
        tone(3, i);
        delay(1);
      }
      noTone(3);
      score1++;
      drawPongScores(score1, score2);
      if (score1 + score2 >= 5) drawPongEnd();
      // erase ball
      TFTscreen.fill(0, 0, 0);                          // black
      TFTscreen.circle(ball_x + 0.5, ball_y + 0.5, 3);      // for rounding instead of truncating
      delay(1000);
      ball_x = 140;
      ball_y = 64;
      ball_angle = -1.5;
      counter = 5;
    }
  }

  /*   no flickering, erasing/drawing only changes
    if (player1 != player1_old) {
      TFTscreen.fill(0, 0, 0);                          // black
      if (player1_old > player1) {
        TFTscreen.rect(1, player1 + 25, 5, player1_old - player1);             // player1 erase
        TFTscreen.fill(0, 255, 0);                          // green
        TFTscreen.rect(1, player1, 5, player1_old - player1);                  // player1 = green
      }
      if (player1_old < player1) {
        TFTscreen.rect(1, player1_old, 5, player1 - player1_old);             // player1 erase
        TFTscreen.fill(0, 255, 0);                          // green
        TFTscreen.rect(1, player1_old + 25, 5, player1 - player1_old);                  // player1 = green
      }
    }
  */
  if (player1 != player1_old) {
    TFTscreen.fill(0, 0, 0);                          // black
    TFTscreen.rect(1, player1_old, 5, 25);             // player1 erase
    TFTscreen.fill(0, 255, 0);                          // green
    TFTscreen.rect(1, player1, 5, 25);                  // player1 = green
  }

  if (player2 != player2_old) {
    TFTscreen.fill(0, 0, 0);                          // black
    TFTscreen.rect(153, player2_old, 5, 25);             // player2 erase
    TFTscreen.fill(0, 0, 255);                          // red
    TFTscreen.rect(153, player2, 5, 25);               // player2 = red
  }

  unsigned long target_duration = max(20 - counter / 2, 3);             // target duration of cycle decreases with counter
  unsigned long elapsed = millis() - timestamp;
  if (target_duration > elapsed) {
    delay(target_duration - elapsed);
  }

}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void drawPongEnd() {

  TFTscreen.setTextSize(2);
  if (score1 > score2) {
    TFTscreen.stroke(0, 255, 0);                // green
    TFTscreen.text("Player 1", 30, 30);
    TFTscreen.text("wins!", 50, 50);
  }
  else {
    TFTscreen.stroke(0, 0, 255);                // red
    TFTscreen.text("Player 2", 30, 30);
    TFTscreen.text("wins!", 50, 50);
  }
  TFTscreen.setTextSize(1);
  TFTscreen.stroke(255, 0, 0);                // blue
  TFTscreen.text("press button to start", 20, 80);

  for (int i = 0; i < 30; i++) {
    tone(3, 2000);
    digitalWrite(2, HIGH);
    delay(20);
    noTone(3);
    digitalWrite(2, LOW);
    delay(20);
  }
  while (1) {                                                               // while wait for reset, short blink
    drawPongScores(score1, score2);
    digitalWrite(2, HIGH);
    delay(20);
    digitalWrite(2, LOW);
  }
  /*
    score1 = 0;
    score2 = 0;
    counter = 0;
    player1 = -1;
    player2 = -1;
    ball_x = 64;
    ball_y = 32;
    drawIntroPong();
    delay(2000);
    TFTscreen.background(0, 0, 0);
    drawPongScores(score1, score2);
    */
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

void checkBallAngle() {
  while (ball_angle > 2 * pi) {
    ball_angle -= 2 * pi;
  }
  while (ball_angle < 0) {
    ball_angle += 2 * pi;
  }

  if (ball_angle > 0 && ball_angle < pi / 4) ball_angle = pi / 4;
  if (ball_angle > 0.75 * pi && ball_angle < pi) ball_angle = 0.75 * pi;
  if (ball_angle > pi && ball_angle < 1.25 * pi) ball_angle = 1.25 * pi;
  if (ball_angle > 1.75 * pi && ball_angle < 2 * pi) ball_angle = 1.75 * pi;
}