/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 ~~~   Arduino Buggy by Quintens Kenny & Raepers Senne     ~
 ~~~                                                       ~
 ~~~  Code written by:   Quintens Kenny                    ~
 ~~~  Design by:         Quintens Kenny & Raepers Senne    ~
 ~~~  Board design by:                    Raepers Senne    ~
 ~~~  Construction by:   Quintens Kenny & Raepers Senne    ~
 ~~~  Coördination by:   Odeurs   Luc                      ~                                                     ~
 ~~~                                                       ~
 ~~~                                                       ~
 ~~~                  ___   ___                            ~
 ~~~      Reset    1 |   `-´   | 28  analog 5              ~
 ~~~         RX    2 |         | 27  analog 4              ~
 ~~~         TX    3 |    A    | 26  analog 3              ~
 ~~~  digital 2    4 |    T    | 25  analog 2              ~
 ~~~      PWM 3    5 |    M    | 24  analog 1              ~
 ~~~  digital 4    6 |    E    | 23  analog 0              ~
 ~~~        VCC    7 |    G    | 22  GND                   ~
 ~~~        GND    8 |    A    | 21  analog reference      ~
 ~~~    crystal    9 |         | 20  VCC                   ~
 ~~~    crystal   10 |    3    | 19  digital 13            ~
 ~~~      PWM 5   11 |    2    | 18  digital 12            ~
 ~~~      PWM 6   12 |    8    | 17  PWM 11                ~
 ~~~  digital 7   13 |         | 16  PWM 10                ~
 ~~~  digital 8   14 |_________| 15  PWM 9                 ~
 ~~~                                                       ~
 ~~~  Copyright 2012:   Kenny Quintens     ~Q-Tronix~      ~
 *///~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~



//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~ - Void Setup - Assignement of the used variables - ~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const int leftdirectionPin = 8;           // Pin nummer voor het richtingssignaal
const int rightdirectionPin = 7;          // Pin nummer voor het richtingssignaal
const int leftpwmPin = 9;                 // Pin nummer voor het PWM signaal   ~Motor control~
const int rightpwmPin = 6;                // Pin nummer voor het PWM signaal
byte leftPwm = 255;                       // Snelheid   "0 - 255"
byte rightPwm = 255;                      // Snelheid   "0 - 255"           =>> Check the minumum speed!!!!
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
int speaker = 13;                         // Luidspreker
int led = 12;                             // Zwaailicht
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
int sensorIR[3] = {A0, A1, A2};        // IR sensors
int threshold = 120;                      // Minimum storings waarde die hij ziet in open lucht
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~ - Void Setup - Assignement of the I/O's - ~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void setup(){
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  pinMode(leftdirectionPin, OUTPUT);     //
  pinMode(rightdirectionPin, OUTPUT);    //                                    ~Motor control~
  pinMode(leftpwmPin, OUTPUT);           //
  pinMode(rightpwmPin, OUTPUT);          //
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  pinMode(speaker, OUTPUT);
  pinMode(led, OUTPUT);
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  int i;
  for(i = 0; i <3; i++){                 // Inlezing van de afstandssensoren
    pinMode(sensorIR[i], INPUT);
  }
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  Serial.begin (9600);

  buggyStart();                          // Melding dat de buggy opgestart is
}



//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~ - Void Loop - Main Programme - ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void loop(){
//=====================================================================
//=                  Read the IRsensor values                         =
//=====================================================================
int sensorValue[3] = {0, 0, 0};
int valueIR;        // Stokage ruimte voor de hoogste IR waarde
int threshold = 120;                      // Minimum storings waarde die hij ziet in open lucht
int sampleTime = 50;                      // Tijd nodig om de sensors in te lezen
int i;
byte highestSensor;

  for(i = 0; i < 2; i++){
    sensorValue[i] = analogRead(sensorIR[i]);

    if(sensorValue[i] > valueIR){
      valueIR = sensorValue[i];
      highestSensor = i;
    }
    if((threshold > valueIR) || (valueIR == 0)){
      highestSensor = 0;
    }
    if(sensorValue[i] <= threshold){
      digitalWrite(speaker, LOW);
      digitalWrite(led, LOW);
      goForward();
    }

    if((sensorValue[i] > threshold) && (sensorValue[i] <= 150)){
      digitalWrite(speaker, LOW);
      motorStop();
      avoidObject();

      digitalWrite(led, HIGH);
      delay(100);
      digitalWrite(led, LOW);
      delay(100);
    }

    if((sensorValue[i] >= 150) && (sensorValue[i] < 500)){
      digitalWrite(speaker, HIGH);
      digitalWrite(led, HIGH);
      delay(100);
      digitalWrite(led, LOW);
      delay(100);
    }

    if(sensorValue[i] >= 500){
      digitalWrite(speaker, LOW);
      digitalWrite(led, LOW);
    }
    delay(sampleTime);
  }
}


//=====================================================================
//=                     Backriding decision                           =
//=====================================================================
void avoidObject(){
  while(1){
    int sensorValue[3] = {0, 0, 0};
    int threshold = 120;
    int i = 0;

    for(i = 0; i < 2; i++){
      sensorValue[i] = analogRead(sensorIR[i]);

      if(sensorValue[i] > 250){
        goBackward();
      }
      else{
        goLeft();
        if(sensorValue[i] <= threshold){
          break;
        }
      }
    }
  }

}

//=====================================================================
//=                      Set turn Right                               =
//=====================================================================
int goRight(){
  setmotorR(LOW, (rightPwm - (rightPwm/2)));
  setmotorL(LOW, leftPwm);

  Serial.println("Go left");
}

//=====================================================================
//=                      Set turn Left                                =
//=====================================================================
int goLeft(){
  setmotorR(LOW, rightPwm);
  setmotorL(LOW, (leftPwm - (leftPwm/2)));

  Serial.println("Go right");
}

//=====================================================================
//=                      Set forward                                  =
//=====================================================================
int goForward(){
  setmotorR(LOW, rightPwm);
  setmotorL(LOW, leftPwm);

  Serial.println("Go forward");
}
//=====================================================================
//=                      Setbackward                                  =
//=====================================================================
int goBackward(){
  setmotorR(HIGH, rightPwm);
  setmotorL(HIGH, leftPwm);

  Serial.println("Go backward");
}
//=====================================================================
//=                      Set stop                                     =
//=====================================================================
int motorStop(){
  setmotorR(LOW, LOW);
  setmotorL(LOW, LOW);

  Serial.println("Stop");
}

//=====================================================================
//=                      Set Right motor                              =
//=====================================================================
void setmotorR(int rightMdirection, int rightpowerValue){
  analogWrite(rightpwmPin, rightpowerValue);
  digitalWrite(rightdirectionPin, rightMdirection);

  Serial.println("Right motor PWM");
  Serial.println('richtpowerValue');
}

//=====================================================================
//=                      Set Left motor                               =
//=====================================================================
void setmotorL(int leftMdirection, int leftpowerValue){
  analogWrite(leftpwmPin, leftpowerValue);
  digitalWrite(leftdirectionPin, leftMdirection);

  Serial.println("Left motor PWM");
  Serial.println(leftpowerValue);
}

//=====================================================================
//=                      Set buggy start                              =
//=====================================================================
void buggyStart(){
  Serial.println("Buggy start");

  digitalWrite(speaker, HIGH);
  digitalWrite(led, HIGH);
  delay(1000);
  digitalWrite(led, LOW);
  delay(1000);
  digitalWrite(speaker, LOW);
}

//=====================================================================