/*
   G.E.A.R.2
   Sample code version (1) 9/25/2016
   by John Olsen aka Robotgoldfish

   With thanks to the authors of the Arduino IDE,
   the adafruit soft servo library and all the tips ,exanple
   code and ideas that made thin possable.


                                 ATtiny85
   _____                         o-\_/--
  (RESET/Analog Input 0 ) Pin 5 |1     8| VCC (+)
         (Analog Input 3) Pin 3 |2     7| Pin 2 (Analog Input 1, SCK)
         (Analog Input 2) Pin 4 |3     6| Pin 1 (PWM, MISO)
                        (-) GND |4     5| Pin 0 (PWM, AREF, MOSI)
                                 -------
  Pin (X) = PB (X)

*/

//pre-set up

#include <Adafruit_SoftServo.h>  // SoftwareServo (works on non PWM pins)

// We demonstrate two servos!
#define SERVO1PIN 0   // Servo control line (orange) on Pin #0 left servo
#define SERVO2PIN 1   // Servo control line (orange) on Pin #1 right servo


Adafruit_SoftServo myServo1, myServo2;  //create TWO servo objects


//The values that stop the CR servos from turning etc.
#define leftStop 90
#define rightStop 88
#define leftFwd 1
#define rightFwd 179
#define leftBak 179
#define rightBak 1

//The Values used in the rangefinding and behaviour routines.
#define longRangeVal 600
#define shortRangeVal 300
#define errorLong 1700
#define errorMin 150
// #define midRange 1300
// ping returnes 0 at < 1in, 150 at 1in, 1300 at midrange, 1600 at ~100in

//Pin declarations.
#define pingPin 2

//Varalable declarations.
long rangeIs = 0;

void setup() {

  // Set up the interrupt that will refresh the servo for us.
  OCR0A = 0xAF;            // any number is OK
  TIMSK |= _BV(OCIE0A);    // Turn on the compare interrupt (below!)


  pinMode(2, OUTPUT); // So the ping rutine both starts and ends with pin 2 as output.

}

void loop()  {

  rangeIs = range();

  myServo1.attach(SERVO1PIN);   // Attach the servo to pin 0 left servo
  myServo2.attach(SERVO2PIN);   // Attach the servo to pin 1 right servo

  if (rangeIs >= longRangeVal) {
    myServo1.write(leftFwd);
    myServo2.write(rightFwd);

    delay(500);

  } else if (rangeIs <= shortRangeVal) {
    myServo1.write(leftBak);
    myServo2.write(rightBak);

  } else {
    myServo1.write(leftBak);
    myServo2.write(rightBak);
    delay(250);
    if (random(11) >= 6) {
      myServo1.write(leftFwd);
      myServo2.write(rightStop);
    } else {
      myServo1.write(leftStop);
      myServo2.write(rightFwd);
    }
    delay(500);
  }
  delay(100);

  myServo1.detach();   // Detach the servo from pin 0
  myServo2.detach();   // Detach the servo from pin 1
  /*     At this point severial peaple just shouted "What".  Its ugly rushed code but because of the
         timer limitations of the ATTINY85 I cant get the PulseIn() and servo commands to play well
         together.  So I turn off the servos when I read the range and then turn them bach on to move.
  */

}

// Functions
long range() {
  // pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  long rangeIs = pulseIn(pingPin, HIGH);
  pinMode(pingPin, OUTPUT);
  rangeIs = constrain(rangeIs, errorMin, errorLong);
  return rangeIs;
}

// The timer interupt routine.  Thanks again Adafruit.
volatile uint8_t counter = 0;
SIGNAL(TIMER0_COMPA_vect) {
  // this gets called every 2 milliseconds
  counter += 2;
  // every 20 milliseconds, refresh the servos!
  if (counter >= 20) {
    counter = 0;
    myServo1.refresh();
    myServo2.refresh();
  }
}