* Gyroskopio */
/* eisagogi bibliothikon */
#include "Wire.h"       
#include "I2Cdev.h"     
#include "MPU6050.h"    

/* arxikopoisoi dedomenon
 * opos sto documentation */
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

struct MyData {
  byte X;
  byte Y;
  byte Z;
};

MyData data;

#define right_echoPin 4
#define right_trigPin 5

/* pins */
#define buzzer 2
#define vibrator_1 3
#define vibrator_2 9
#define vibrator_3 10
#define loop_button 13

/* arxikopioiseis */
byte d_old = 15;
byte d_new = 15;

void setup() {
  Serial.begin(9600);
  pinMode(right_echoPin, INPUT);
  pinMode(right_trigPin, OUTPUT);
  pinMode(loop_button, INPUT);

  pinMode(vibrator_1, OUTPUT);
  pinMode(vibrator_2, OUTPUT);
  pinMode(vibrator_3, OUTPUT);
  pinMode(buzzer, OUTPUT);

  Wire.begin();
  mpu.initialize();
}
  
void loop() {
  byte d=aisthitirio_right(); /* pairnoume apostasi */
  Serial.print("Apostasi: ");
  Serial.println(d);
  delay(100);
  if (d > 9) return; /* skip se periptwsi pou apostasi einai ano tou 9*/
  if (digitalRead(loop_button) == 0) return; /* skip an to loop den einai patimeno */

  /* apothikevoume nea apostasi gia sygkrisi */
  d_old = d_new;
  d_new = d;
  if (d_old <= d_new) return; /* sygkrisi */

  /* pairnoume moires axonon apo to gyroskopio */
  /* symfona me to documentation */
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  data.X = map(ax, -17000, 17000, 0, 255 ); // X axis data
  data.Y = map(ay, -17000, 17000, 0, 255); 
  data.Z = map(az, -17000, 17000, 0, 255);  // Y axis data
  delay(500);
  Serial.print("X=");
  Serial.print(data.X);
  Serial.print(" ");
  Serial.print("Y=");
  Serial.print(data.Y);
  Serial.print(" ");
  Serial.print("Z="); 
  Serial.print(data.Z);
  if ((data.Y > 90 && data.Y < 140) && (data.X > 100 && data.X < 140)) {
    Serial.print(" - [Katastasi leitourgias] - ");
  } else {
    Serial.println("  [Ektos leitourgias] - ");
    return;
  }

  /* i apostasi einai aparaitita mikroteri tou 9 logo tou eleghou pou kaname */
  if (d > 6) { /* an apostasi > 6, xtipaei o 1os donitis me 180 */
    analogWrite(vibrator_1, 180);
    delay(400);
    analogWrite(vibrator_1, 0);
    delay(500);  
  } else if (d > 3) {  /* an apostasi > 3, xtipaei o 1os kai o 2os me 220 */
    analogWrite(vibrator_1, 220);
    analogWrite(vibrator_2, 220);
    delay(600);
    analogWrite(vibrator_1, 0);
    analogWrite(vibrator_2, 0);
    delay(500);
  } else { /* an apostasi <= 3, xtipane oloi oi donites me 255 kai to buzzer */
    digitalWrite(buzzer, HIGH);
    analogWrite(vibrator_1, 255);
    analogWrite(vibrator_2, 255);
    analogWrite(vibrator_3, 255);
    delay(800);
    digitalWrite(buzzer, LOW);
    analogWrite(vibrator_1, 0);
    analogWrite(vibrator_2, 0);
    analogWrite(vibrator_3, 0);
    delay(500); 
  } /* den yparxei periptosi i apostasi na einai pano apo to 9 */
}

// Auth h synarthsh epistrefei thn apostash toy deksioy empodioy se cm h 50000 an den yparxei empodio
byte aisthitirio_right()
{
  digitalWrite(right_trigPin, LOW);                   // Set the trigger pin to low for 2uS
  delayMicroseconds(2);
  digitalWrite(right_trigPin, HIGH);                  // Send a 10uS high to trigger ranging
  delayMicroseconds(10);
  digitalWrite(right_trigPin, LOW);                   // Send pin low again
  int distance = pulseIn(right_echoPin, HIGH);        // Read in times pulse
  distance= distance/58;   
  byte mes=(byte) distance;  
  delay(50);                                    // Wait 50mS before next ranging
  return(mes);                     
}