/*
 * Copyright 2018 Heidi Hartikainen
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation 
 * files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, 
 * modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software 
 * is furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 
 IN THE SOFTWARE.*/

#include <Ultrasonic.h>                   //An ultrasonic library by Eric Simoes at: https://github.com/ErickSimoes/Ultrasonic 
Ultrasonic ultrasonic(2,3, 40000UL);      // (Trig PIN,Echo PIN, Timeout). If there is no object in range, the library will lock-up as it waits for the return pulse. You can change how long to wait by setting a timeout (in microseconds)
int Range;
int dist; 

//h-bridge Motor1 pins on Heidi's turtle robot w. Attinty44
#define RightMotorForward 7     //Pin 7 in arduino correcponds to PA7 (pin6) on attiny44
#define RightMotorReverse 9     //Pin 9 in aduino corresponds to PB1 (pin3) on attiny44
//Hbridge motor Motor2 pins  on Heidi's turtle robot w. Attinty44
#define LeftMotorForward 8    //Pin 8 in adruino corresponds to PB2 (pin5) on attiny 44
#define LeftMotorReverse 10     //Pin 10 in adruino corresponds to PB0 (pin2) on attiny 44
//Sonar pins on Heidi's turtle robot w. Attinty44
int trigger_pin = 2;          //Pin 2 in adruino corresponds PA2 (pin11) on attiny 44
int echo_pin = 3;             //Pin 3 in adruino corresponds to PA3 (pin10) on attiny 44

  void setup()
  {
  // put your setup code here, to run once:
    pinMode(RightMotorForward, OUTPUT);  // initialize the  pin as an output.
    pinMode(RightMotorReverse, OUTPUT);  // initialize the  pin as an output.
    pinMode(LeftMotorForward, OUTPUT);  // initialize the  pin as an output.
    pinMode(LeftMotorReverse, OUTPUT);  // initialize the  pin as an output.
  }
  void loop()
  {   
   // put your main code here, to run repeatedly:
    dist = 30;                          //The min distance to an object before turning right (cm)
    Range = ultrasonic.distanceRead();  //distanceRead returns the value in cm. 

   while (Range > dist || Range==0)       // If we are more than 30cm from a wall or 0cm (which would mean out of range as the sonar only can read up to 400cm) go forward
   {
   analogWrite(RightMotorForward, 255);   // turn the Right Motor ON
   analogWrite(LeftMotorForward, 195);   // turn the Left Motor ON. The left motor was more powerful than the right one, so the value was adjusted
   Range=ultrasonic.distanceRead();      //Get the range again 
   }
   
    analogWrite(RightMotorForward, 0);    // turn the Right Motor OFF
    analogWrite(LeftMotorForward, 0);   // turn the Left Motor off 
    delay(3000);                        //wait for 3 seconds
    
    while (ultrasonic.distanceRead()<=dist)     //If there is less than 30cm to a wall - Turn right. 
    {       
    //Turn right.        
    analogWrite(RightMotorForward, 85);    // turn the Right Motor on, quite slow
    analogWrite(LeftMotorForward, 255);   // turn the Left Motor ON (full speed)
    delay(2000);                          //wait for 2 seconds
    }

}