//GoodBoy Robot Dog Code
//Written by Instructables user dentdentarthurdent
//Published 25/05/20

#ifndef NewPing_h

	#define NewPing_h

	#if defined (ARDUINO) && ARDUINO >= 100
		#include <Arduino.h>
	#else
		#include <WProgram.h>
		#if defined (PARTICLE)
			#include <SparkIntervalTimer.h>
		#else
			#include <pins_arduino.h>
		#endif
	#endif

	#if defined (__AVR__)
		#include <avr/io.h>
		#include <avr/interrupt.h>
	#endif

	// Shouldn't need to change these values unless you have a specific need to do so.
	#define MAX_SENSOR_DISTANCE 500 // Maximum sensor distance can be as high as 500cm, no reason to wait for ping longer than sound takes to travel this distance and back. Default=500
	#define US_ROUNDTRIP_CM 57      // Microseconds (uS) it takes sound to travel round-trip 1cm (2cm total), uses integer to save compiled code space. Default=57
	#define US_ROUNDTRIP_IN 146     // Microseconds (uS) it takes sound to travel round-trip 1 inch (2 inches total), uses integer to save compiled code space. Defalult=146
	#define ONE_PIN_ENABLED true    // Set to "false" to disable one pin mode which saves around 14-26 bytes of binary size. Default=true
	#define ROUNDING_ENABLED false  // Set to "true" to enable distance rounding which also adds 64 bytes to binary size. Default=false
	#define URM37_ENABLED false     // Set to "true" to enable support for the URM37 sensor in PWM mode. Default=false
	#define TIMER_ENABLED true      // Set to "false" to disable the timer ISR (if getting "__vector_7" compile errors set this to false). Default=true

	// Probably shouldn't change these values unless you really know what you're doing.
	#define NO_ECHO 0               // Value returned if there's no ping echo within the specified MAX_SENSOR_DISTANCE or max_cm_distance. Default=0
	#define MAX_SENSOR_DELAY 5800   // Maximum uS it takes for sensor to start the ping. Default=5800
	#define ECHO_TIMER_FREQ 24      // Frequency to check for a ping echo (every 24uS is about 0.4cm accuracy). Default=24
	#define PING_MEDIAN_DELAY 29000 // Microsecond delay between pings in the ping_median method. Default=29000
	#define PING_OVERHEAD 5         // Ping overhead in microseconds (uS). Default=5
	#define PING_TIMER_OVERHEAD 13  // Ping timer overhead in microseconds (uS). Default=13
	#if URM37_ENABLED == true
		#undef  US_ROUNDTRIP_CM
		#undef  US_ROUNDTRIP_IN
		#define US_ROUNDTRIP_CM 50  // Every 50uS PWM signal is low indicates 1cm distance. Default=50
		#define US_ROUNDTRIP_IN 127 // If 50uS is 1cm, 1 inch would be 127uS (50 x 2.54 = 127). Default=127
	#endif

	// Conversion from uS to distance (round result to nearest cm or inch).
	#define NewPingConvert(echoTime, conversionFactor) (max(((unsigned int)echoTime + conversionFactor / 2) / conversionFactor, (echoTime ? 1 : 0)))

	// Detect non-AVR microcontrollers (Teensy 3.x, Arduino DUE, etc.) and don't use port registers or timer interrupts as required.
	#if (defined (__arm__) && (defined (TEENSYDUINO) || defined (PARTICLE)))
		#undef  PING_OVERHEAD
		#define PING_OVERHEAD 1
		#undef  PING_TIMER_OVERHEAD
		#define PING_TIMER_OVERHEAD 1
		#define DO_BITWISE true
	#elif !defined (__AVR__)
		#undef  PING_OVERHEAD
		#define PING_OVERHEAD 1
		#undef  PING_TIMER_OVERHEAD
		#define PING_TIMER_OVERHEAD 1
		#undef  TIMER_ENABLED
		#define TIMER_ENABLED false
		#define DO_BITWISE false
	#else
		#define DO_BITWISE true
	#endif

	// Disable the timer interrupts when using ATmega128 and all ATtiny microcontrollers.
	#if defined (__AVR_ATmega128__) || defined (__AVR_ATtiny24__) || defined (__AVR_ATtiny44__) || defined (__AVR_ATtiny441__) || defined (__AVR_ATtiny84__) || defined (__AVR_ATtiny841__) || defined (__AVR_ATtiny25__) || defined (__AVR_ATtiny45__) || defined (__AVR_ATtiny85__) || defined (__AVR_ATtiny261__) || defined (__AVR_ATtiny461__) || defined (__AVR_ATtiny861__) || defined (__AVR_ATtiny43U__)
		#undef  TIMER_ENABLED
		#define TIMER_ENABLED false
	#endif

	// Define timers when using ATmega8, ATmega16, ATmega32 and ATmega8535 microcontrollers.
	#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__)
		#define OCR2A OCR2
		#define TIMSK2 TIMSK
		#define OCIE2A OCIE2
	#endif

	class NewPing {
		public:
			NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance = MAX_SENSOR_DISTANCE);
			unsigned int ping(unsigned int max_cm_distance = 0);
			unsigned long ping_cm(unsigned int max_cm_distance = 0);
			unsigned long ping_in(unsigned int max_cm_distance = 0);
			unsigned long ping_median(uint8_t it = 5, unsigned int max_cm_distance = 0);
			static unsigned int convert_cm(unsigned int echoTime);
			static unsigned int convert_in(unsigned int echoTime);
	#if TIMER_ENABLED == true
			void ping_timer(void (*userFunc)(void), unsigned int max_cm_distance = 0);
			boolean check_timer();
			unsigned long ping_result;
			static void timer_us(unsigned int frequency, void (*userFunc)(void));
			static void timer_ms(unsigned long frequency, void (*userFunc)(void));
			static void timer_stop();
	#endif
		private:
			boolean ping_trigger();
			void set_max_distance(unsigned int max_cm_distance);
	#if TIMER_ENABLED == true
			boolean ping_trigger_timer(unsigned int trigger_delay);
			boolean ping_wait_timer();
			static void timer_setup();
			static void timer_ms_cntdwn();
	#endif
	#if DO_BITWISE == true
			uint8_t _triggerBit;
			uint8_t _echoBit;
		#if defined(PARTICLE)
			#if !defined(portModeRegister)
				#if defined (STM32F10X_MD)
					#define portModeRegister(port)     ( &(port->CRL) )
				#elif defined (STM32F2XX)
					#define portModeRegister(port)     ( &(port->MODER) )
				#endif
			#endif
			volatile uint32_t *_triggerOutput;
			volatile uint32_t *_echoInput;
			volatile uint32_t *_triggerMode;
		#else
			volatile uint8_t *_triggerOutput;
			volatile uint8_t *_echoInput;
			volatile uint8_t *_triggerMode;
		#endif
	#else
			uint8_t _triggerPin;
			uint8_t _echoPin;
	#endif
			unsigned int _maxEchoTime;
			unsigned long _max_time;
	};


#endif

// ---------------------------------------------------------------------------
// NewPing constructor
// ---------------------------------------------------------------------------

NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance) {
#if DO_BITWISE == true
	_triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin.
	_echoBit = digitalPinToBitMask(echo_pin);       // Get the port register bitmask for the echo pin.

	_triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin.
	_echoInput = portInputRegister(digitalPinToPort(echo_pin));         // Get the input port register for the echo pin.

	_triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin.
#else
	_triggerPin = trigger_pin;
	_echoPin = echo_pin;
#endif

	set_max_distance(max_cm_distance); // Call function to set the max sensor distance.

#if (defined (__arm__) && (defined (TEENSYDUINO) || defined(PARTICLE))) || DO_BITWISE != true
	pinMode(echo_pin, INPUT);     // Set echo pin to input (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
	pinMode(trigger_pin, OUTPUT); // Set trigger pin to output (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
#endif

#if defined (ARDUINO_AVR_YUN)
	pinMode(echo_pin, INPUT);     // Set echo pin to input for the Arduino Yun, not sure why it doesn't default this way.
#endif

#if ONE_PIN_ENABLED != true && DO_BITWISE == true
	*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
}


// ---------------------------------------------------------------------------
// Standard ping methods
// ---------------------------------------------------------------------------

unsigned int NewPing::ping(unsigned int max_cm_distance) {
	if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance.

	if (!ping_trigger()) return NO_ECHO; // Trigger a ping, if it returns false, return NO_ECHO to the calling function.

#if URM37_ENABLED == true
	#if DO_BITWISE == true
		while (!(*_echoInput & _echoBit))             // Wait for the ping echo.
	#else
		while (!digitalRead(_echoPin))                // Wait for the ping echo.
	#endif
			if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance.
#else
	#if DO_BITWISE == true
		while (*_echoInput & _echoBit)                // Wait for the ping echo.
	#else
		while (digitalRead(_echoPin))                 // Wait for the ping echo.
	#endif
			if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance.
#endif

	return (micros() - (_max_time - _maxEchoTime) - PING_OVERHEAD); // Calculate ping time, include overhead.
}


unsigned long NewPing::ping_cm(unsigned int max_cm_distance) {
	unsigned long echoTime = NewPing::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS.
#if ROUNDING_ENABLED == false
	return (echoTime / US_ROUNDTRIP_CM);              // Call the ping method and returns the distance in centimeters (no rounding).
#else
	return NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}


unsigned long NewPing::ping_in(unsigned int max_cm_distance) {
	unsigned long echoTime = NewPing::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS.
#if ROUNDING_ENABLED == false
	return (echoTime / US_ROUNDTRIP_IN);              // Call the ping method and returns the distance in inches (no rounding).
#else
	return NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}


unsigned long NewPing::ping_median(uint8_t it, unsigned int max_cm_distance) {
	unsigned int uS[it], last;
	uint8_t j, i = 0;
	unsigned long t;
	uS[0] = NO_ECHO;

	while (i < it) {
		t = micros();                  // Start ping timestamp.
		last = ping(max_cm_distance);  // Send ping.

		if (last != NO_ECHO) {         // Ping in range, include as part of median.
			if (i > 0) {               // Don't start sort till second ping.
				for (j = i; j > 0 && uS[j - 1] < last; j--) // Insertion sort loop.
					uS[j] = uS[j - 1];                      // Shift ping array to correct position for sort insertion.
			} else j = 0;              // First ping is sort starting point.
			uS[j] = last;              // Add last ping to array in sorted position.
			i++;                       // Move to next ping.
		} else it--;                   // Ping out of range, skip and don't include as part of median.

		if (i < it && micros() - t < PING_MEDIAN_DELAY)
			delay((PING_MEDIAN_DELAY + t - micros()) / 1000); // Millisecond delay between pings.

	}
	return (uS[it >> 1]); // Return the ping distance median.
}


// ---------------------------------------------------------------------------
// Standard and timer interrupt ping method support functions (not called directly)
// ---------------------------------------------------------------------------

boolean NewPing::ping_trigger() {
#if DO_BITWISE == true
	#if ONE_PIN_ENABLED == true
		*_triggerMode |= _triggerBit;  // Set trigger pin to output.
	#endif

	*_triggerOutput &= ~_triggerBit;   // Set the trigger pin low, should already be low, but this will make sure it is.
	delayMicroseconds(4);              // Wait for pin to go low.
	*_triggerOutput |= _triggerBit;    // Set trigger pin high, this tells the sensor to send out a ping.
	delayMicroseconds(10);             // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS.
	*_triggerOutput &= ~_triggerBit;   // Set trigger pin back to low.

	#if ONE_PIN_ENABLED == true
		*_triggerMode &= ~_triggerBit; // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin).
	#endif

	#if URM37_ENABLED == true
		if (!(*_echoInput & _echoBit)) return false;            // Previous ping hasn't finished, abort.
		_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
		while (*_echoInput & _echoBit)                          // Wait for ping to start.
			if (micros() > _max_time) return false;             // Took too long to start, abort.
	#else
		if (*_echoInput & _echoBit) return false;               // Previous ping hasn't finished, abort.
		_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
		while (!(*_echoInput & _echoBit))                       // Wait for ping to start.
			if (micros() > _max_time) return false;             // Took too long to start, abort.
	#endif
#else
	#if ONE_PIN_ENABLED == true
		pinMode(_triggerPin, OUTPUT); // Set trigger pin to output.
	#endif
	
	digitalWrite(_triggerPin, LOW);   // Set the trigger pin low, should already be low, but this will make sure it is.
	delayMicroseconds(4);             // Wait for pin to go low.
	digitalWrite(_triggerPin, HIGH);  // Set trigger pin high, this tells the sensor to send out a ping.
	delayMicroseconds(10);            // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS.
	digitalWrite(_triggerPin, LOW);   // Set trigger pin back to low.

	#if ONE_PIN_ENABLED == true
		pinMode(_triggerPin, INPUT);  // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin).
	#endif

	#if URM37_ENABLED == true
		if (!digitalRead(_echoPin)) return false;               // Previous ping hasn't finished, abort.
		_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
		while (digitalRead(_echoPin))                           // Wait for ping to start.
			if (micros() > _max_time) return false;             // Took too long to start, abort.
	#else
		if (digitalRead(_echoPin)) return false;                // Previous ping hasn't finished, abort.
		_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
		while (!digitalRead(_echoPin))                          // Wait for ping to start.
			if (micros() > _max_time) return false;             // Took too long to start, abort.
	#endif
#endif

	_max_time = micros() + _maxEchoTime; // Ping started, set the time-out.
	return true;                         // Ping started successfully.
}


void NewPing::set_max_distance(unsigned int max_cm_distance) {
#if ROUNDING_ENABLED == false
	_maxEchoTime = min(max_cm_distance + 1, (unsigned int) MAX_SENSOR_DISTANCE + 1) * US_ROUNDTRIP_CM; // Calculate the maximum distance in uS (no rounding).
#else
	_maxEchoTime = min(max_cm_distance, (unsigned int) MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS.
#endif
}


#if TIMER_ENABLED == true && DO_BITWISE == true

	// ---------------------------------------------------------------------------
	// Timer interrupt ping methods (won't work with ATmega128, ATtiny and most non-AVR microcontrollers)
	// ---------------------------------------------------------------------------

	void NewPing::ping_timer(void (*userFunc)(void), unsigned int max_cm_distance) {
		if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance.

		if (!ping_trigger()) return;         // Trigger a ping, if it returns false, return without starting the echo timer.
		timer_us(ECHO_TIMER_FREQ, userFunc); // Set ping echo timer check every ECHO_TIMER_FREQ uS.
	}


	boolean NewPing::check_timer() {
		if (micros() > _max_time) { // Outside the time-out limit.
			timer_stop();           // Disable timer interrupt
			return false;           // Cancel ping timer.
		}

	#if URM37_ENABLED == false
		if (!(*_echoInput & _echoBit)) { // Ping echo received.
	#else
		if (*_echoInput & _echoBit) {    // Ping echo received.
	#endif
			timer_stop();                // Disable timer interrupt
			ping_result = (micros() - (_max_time - _maxEchoTime) - PING_TIMER_OVERHEAD); // Calculate ping time including overhead.
			return true;                 // Return ping echo true.
		}

		return false; // Return false because there's no ping echo yet.
	}


	// ---------------------------------------------------------------------------
	// Timer2/Timer4 interrupt methods (can be used for non-ultrasonic needs)
	// ---------------------------------------------------------------------------

	// Variables used for timer functions
	void (*intFunc)();
	void (*intFunc2)();
	unsigned long _ms_cnt_reset;
	volatile unsigned long _ms_cnt;
	#if defined(__arm__) && (defined (TEENSYDUINO) || defined(PARTICLE))
		IntervalTimer itimer;
	#endif


	void NewPing::timer_us(unsigned int frequency, void (*userFunc)(void)) {
		intFunc = userFunc; // User's function to call when there's a timer event.
		timer_setup();      // Configure the timer interrupt.

	#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
		OCR4C = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit.
		TIMSK4 = (1<<TOIE4);                  // Enable Timer4 interrupt.
	#elif defined (__arm__) && defined (TEENSYDUINO) // Timer for Teensy 3.x
		itimer.begin(userFunc, frequency);           // Really simple on the Teensy 3.x, calls userFunc every 'frequency' uS.
	#elif defined (__arm__) && defined (PARTICLE)    // Timer for Particle devices
		itimer.begin(userFunc, frequency, uSec);     // Really simple on the Particle, calls userFunc every 'frequency' uS.
	#else
		OCR2A = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit.
		TIMSK2 |= (1<<OCIE2A);                // Enable Timer2 interrupt.
	#endif
	}


	void NewPing::timer_ms(unsigned long frequency, void (*userFunc)(void)) {
		intFunc = NewPing::timer_ms_cntdwn;  // Timer events are sent here once every ms till user's frequency is reached.
		intFunc2 = userFunc;                 // User's function to call when user's frequency is reached.
		_ms_cnt = _ms_cnt_reset = frequency; // Current ms counter and reset value.
		timer_setup();                       // Configure the timer interrupt.

	#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
		OCR4C = 249;           // Every count is 4uS, so 1ms = 250 counts - 1.
		TIMSK4 = (1<<TOIE4);   // Enable Timer4 interrupt.
	#elif defined (__arm__) && defined (TEENSYDUINO)        // Timer for Teensy 3.x
		itimer.begin(NewPing::timer_ms_cntdwn, 1000);       // Set timer to 1ms (1000 uS).
	#elif defined (__arm__) && defined (PARTICLE)           // Timer for Particle
		itimer.begin(NewPing::timer_ms_cntdwn, 1000, uSec); // Set timer to 1ms (1000 uS).
	#else
		OCR2A = 249;           // Every count is 4uS, so 1ms = 250 counts - 1.
		TIMSK2 |= (1<<OCIE2A); // Enable Timer2 interrupt.
	#endif
	}


	void NewPing::timer_stop() { // Disable timer interrupt.
	#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
		TIMSK4 = 0;
	#elif defined (__arm__) && (defined (TEENSYDUINO) || defined (PARTICLE)) // Timer for Teensy 3.x & Particle
		itimer.end();
	#else
		TIMSK2 &= ~(1<<OCIE2A);
	#endif
	}


	// ---------------------------------------------------------------------------
	// Timer2/Timer4 interrupt method support functions (not called directly)
	// ---------------------------------------------------------------------------

	void NewPing::timer_setup() {
	#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
		timer_stop(); // Disable Timer4 interrupt.
		TCCR4A = TCCR4C = TCCR4D = TCCR4E = 0;
		TCCR4B = (1<<CS42) | (1<<CS41) | (1<<CS40) | (1<<PSR4); // Set Timer4 prescaler to 64 (4uS/count, 4uS-1020uS range).
		TIFR4 = (1<<TOV4);
		TCNT4 = 0;    // Reset Timer4 counter.
	#elif defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__) // Alternate timer commands for certain microcontrollers.
		timer_stop();                 // Disable Timer2 interrupt.
		ASSR &= ~(1<<AS2);            // Set clock, not pin.
		TCCR2 = (1<<WGM21 | 1<<CS22); // Set Timer2 to CTC mode, prescaler to 64 (4uS/count, 4uS-1020uS range).
		TCNT2 = 0;                    // Reset Timer2 counter.
	#elif defined (__arm__) && (defined (TEENSYDUINO) || defined (PARTICLE))
		timer_stop(); // Stop the timer.
	#else
		timer_stop();        // Disable Timer2 interrupt.
		ASSR &= ~(1<<AS2);   // Set clock, not pin.
		TCCR2A = (1<<WGM21); // Set Timer2 to CTC mode.
		TCCR2B = (1<<CS22);  // Set Timer2 prescaler to 64 (4uS/count, 4uS-1020uS range).
		TCNT2 = 0;           // Reset Timer2 counter.
	#endif
	}


	void NewPing::timer_ms_cntdwn() {
		if (!_ms_cnt--) {            // Count down till we reach zero.
			intFunc2();              // Scheduled time reached, run the main timer event function.
			_ms_cnt = _ms_cnt_reset; // Reset the ms timer.
		}
	}

	#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
	ISR(TIMER4_OVF_vect) {
		intFunc(); // Call wrapped function.
	}
	#elif defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__) // Alternate timer commands for certain microcontrollers.
	ISR(TIMER2_COMP_vect) {
		intFunc(); // Call wrapped function.
	}
	#elif defined (__arm__)
		// Do nothing...
	#else
	ISR(TIMER2_COMPA_vect) {
		intFunc(); // Call wrapped function.
	}
	#endif


#endif


// ---------------------------------------------------------------------------
// Conversion methods (rounds result to nearest cm or inch).
// ---------------------------------------------------------------------------

unsigned int NewPing::convert_cm(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
	return (echoTime / US_ROUNDTRIP_CM);              // Convert uS to centimeters (no rounding).
#else
	return NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}


unsigned int NewPing::convert_in(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
	return (echoTime / US_ROUNDTRIP_IN);              // Convert uS to inches (no rounding).
#else
	return NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}

//#include <NewPing.h>
#include <Servo.h>

#define trigPin 5
#define echoPin 4
#define trigPin2 3
#define echoPin2 2
#define MAX_DISTANCE 350

NewPing sonar_l(trigPin, echoPin, MAX_DISTANCE);
float duration, distance;
NewPing sonar_r(trigPin2, echoPin2, MAX_DISTANCE);
float duration2, distance2;

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;
Servo myservo7;
Servo myservo8;

int pos1 = 65;
int pos2 = 80;
int pos3 = 120;
int pos4 = 80;
int pos5 = 45;
int pos6 = 80;
int pos7 = 125;
int pos8 = 90;

int steps = 0;

int sensorPin = A1;
const int led1 = 12;
const int led2 = 13;
int counter = 0;
int strides;
int milliseconds;

void setup() {
  myservo1.attach(13);
  myservo2.attach(12);
  myservo3.attach(11);
  myservo4.attach(10);
  myservo5.attach(9);
  myservo6.attach(8);
  myservo7.attach(7);
  myservo8.attach(6);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  delay(2000);
  Serial.begin(9600);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//GOODBOY BEHAVIOUR
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void loop() {
  milliseconds = 10000;
  standstill(milliseconds);
  strides = 20;
  stepforward(strides);
  pawtrick();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//STANDSTILL FUNCTION
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

int standstill(int x) {
  //NEUTRAL POSITION
  pos1 = 65;
  pos2 = 80;
  pos3 = 120;
  pos4 = 80;
  pos5 = 42;
  pos6 = 95;
  pos7 = 128;
  pos8 = 75;
  myservo1.write(pos1);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  myservo5.write(pos5);
  myservo6.write(pos6);
  myservo7.write(pos7);
  myservo8.write(pos8);
  delay(x);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//PAW TRICK FUNCTION
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

int pawtrick() {
  pos1 = 65;
  pos2 = 80;
  pos3 = 120;
  pos4 = 80;
  pos5 = 42;
  pos6 = 80;
  pos7 = 125;
  pos8 = 90;

  myservo1.write(65);
  myservo2.write(80);
  myservo3.write(120);
  myservo4.write(80);
  myservo5.write(45);
  myservo6.write(80);
  myservo7.write(125);
  myservo8.write(90);

  int c = analogRead(sensorPin);

  if (c < 900) {
    Serial.println("battery low");
    digitalWrite(led1, LOW);
    counter = counter + 1;
  }
  else {
    digitalWrite(led1, HIGH);
    counter = 0;
  }
  delay(50);

  if (counter > 50) {
    digitalWrite(led2, HIGH);
    for (pos1 = 65; pos1 <= 85; pos1 += 1) {
      myservo1.write(pos1);
      pos2 = pos2 + 1;
      myservo2.write(pos2);
      delay(25);
    }
    for (pos1 = 85; pos1 <= 110; pos1 += 1) {
      myservo1.write(pos1);
      delay(25);
    }
    for (pos2 = 100; pos2 >= 50; pos2 -= 1) {
      myservo2.write(pos2);
      delay(25);
    }
    for (pos1 = 110; pos1 >= 100; pos1 -= 1) {
      myservo1.write(pos1);
      delay(25);
    }
    delay(3000);
    for (pos1 = 100; pos1 <= 110; pos1 += 1) {
      myservo1.write(pos1);
      delay(25);
    }
    for (pos2 = 50; pos2 <= 100; pos2 += 1) {
      myservo2.write(pos2);
      delay(25);
    }
    for (pos1 = 110; pos1 >= 85; pos1 -= 1) {
      myservo1.write(pos1);
      delay(25);
    }
    for (pos1 = 85; pos1 >= 55; pos1 -= 1) {
      myservo1. write(pos1);
      pos2 = pos2 - 1;
      myservo2.write(pos2);
      delay(25);
    }
    for (pos1 = 55; pos1 <= 65; pos1 += 1) {
      myservo1.write(pos1);
      pos2 = pos2 + 1;
      myservo2.write(pos2);
      delay(25);
    }
    delay(5000);
    digitalWrite(led2, LOW);
  }
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//STRIDES FUNCTION
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

int stepforward(int y) {
  int i = 5;

  //NEUTRAL POSITION
  pos1 = 65;
  pos2 = 80;
  pos3 = 120;
  pos4 = 80;
  pos5 = 42;
  pos6 = 95;
  pos7 = 128;
  pos8 = 75;
  delay(i);

  for (steps = 1; steps <= y; steps += 1) {

    duration = sonar_r.ping();
    distance = (duration / 2) * 0.0343;
    duration2 = sonar_l.ping();
    distance2 = (duration2 / 2) * 0.0343;
    Serial.println("distance1 : ");
Serial.print(distance);
 Serial.print("distance2 : ");
Serial.println(distance2);
    if (distance > 10 && distance2 > 10) {

      //LEFT FRONT STEP FORWARDS MOVEMENT
      for (counter = 1; counter <= 20; counter += 1) {
        pos1 = pos1 + 1;
        myservo1.write(pos1);
        delay(i);
      }
      for (counter = 1; counter <= 14; counter += 1) {
        pos2 = pos2 - 1;
        myservo2.write(pos2);
        delay(i);
      }
      for (counter = 1; counter <= 27; counter += 1) {
        pos1 = pos1 - 1;
        myservo1.write(pos1);
        delay(i);
      }

      //LEFT FRONT STEP FORWARDS (UNDER TEST)
      pos1 = 58;
      pos2 = 66;
      pos3 = 120;
      pos4 = 80;
      pos5 = 42;
      pos6 = 95;
      pos7 = 128;
      pos8 = 75;
      myservo1.write(pos1);
      myservo2.write(pos2);
      myservo3.write(pos3);
      myservo4.write(pos4);
      myservo5.write(pos5);
      myservo6.write(pos6);
      myservo7.write(pos7);
      myservo8.write(pos8);
      delay(300);

      //RIGHT FRONT STEP FORWARDS MOVEMENT
      for (counter = 1; counter <= 20; counter += 1) {
        pos3 = pos3 - 1;
        myservo3.write(pos3);
        delay(i);
      }
      for (counter = 1; counter <= 14; counter += 1) {
        pos4 = pos4 + 1;
        myservo4.write(pos4);
        delay(i);
      }
      for (counter = 1; counter <= 27; counter += 1) {
        pos3 = pos3 + 1;
        myservo3.write(pos3);
        delay(i);
      }

      //RIGHT FRONT STEP FORWARDS (UNDER TEST)
      pos1 = 58;
      pos2 = 66;
      pos3 = 127;
      pos4 = 94;
      pos5 = 42;
      pos6 = 95;
      pos7 = 128;
      pos8 = 75;
      myservo1.write(pos1);
      myservo2.write(pos2);
      myservo3.write(pos3);
      myservo4.write(pos4);
      myservo5.write(pos5);
      myservo6.write(pos6);
      myservo7.write(pos7);
      myservo8.write(pos8);
      delay(300);

      //COG SHIFT MOVEMENT
      for (counter = 1; counter <= 7; counter += 1) {
        pos1 = pos1 + 1;
        pos2 = pos2 + 2;
        pos3 = pos3 - 1;
        pos4 = pos4 - 2;
        pos5 = pos5 - 1;
        pos6 = pos6 + 1;
        pos7 = pos7 + 1;
        pos8 = pos8 - 1;
        myservo1.write(pos1);
        myservo2.write(pos2);
        myservo3.write(pos3);
        myservo4.write(pos4);
        myservo5.write(pos5);
        myservo6.write(pos6);
        myservo7.write(pos7);
        myservo8.write(pos8);
        delay(90);
      }
      for (counter = 1; counter <= 2; counter += 1) {
        pos6 = pos6 + 1;
        pos8 = pos8 - 1;
        myservo6.write(pos6);
        myservo8.write(pos8);
        delay(90);
      }

      //COG SHIFT FROM REAR TO FRONT (UNDER TEST)
      pos1 = 65;
      pos2 = 80;
      pos3 = 120;
      pos4 = 80;
      pos5 = 35;
      pos6 = 104;
      pos7 = 135;
      pos8 = 66;
      myservo1.write(pos1);
      myservo2.write(pos2);
      myservo3.write(pos3);
      myservo4.write(pos4);
      myservo5.write(pos5);
      myservo6.write(pos6);
      myservo7.write(pos7);
      myservo8.write(pos8);
      delay(300);

      //LEFT REAR STEP FORWARDS MOVEMENT
      for (counter = 1; counter <= 10; counter += 1) {
        pos6 = pos6 + 1;
        myservo6.write(pos6);
        delay(i);
      }
      for (counter = 1; counter <= 30; counter += 1) {
        pos5 = pos5 + 1;
        myservo5.write(pos5);
        delay(i);
      }
      for (counter = 1; counter <= 19; counter += 1) {
        pos6 = pos6 - 1;
        myservo6.write(pos6);
        delay(i);
      }
      for (counter = 1; counter <= 28; counter += 1) {
        pos5 = pos5 - 1;
        myservo5.write(pos5);
        delay(i);
      }
      for (counter = 1; counter <= 5; counter += 1) {
        pos5 = pos5 + 1;
        myservo5.write(pos5);
        delay(i);
      }

      //LEFT REAR STEP FORWARDS
      pos1 = 65;
      pos2 = 80;
      pos3 = 120;
      pos4 = 80;
      pos5 = 42;
      pos6 = 95;
      pos7 = 135;
      pos8 = 66;
      myservo1.write(pos1);
      myservo2.write(pos2);
      myservo3.write(pos3);
      myservo4.write(pos4);
      myservo5.write(pos5);
      myservo6.write(pos6);
      myservo7.write(pos7);
      myservo8.write(pos8);
      delay(300);

      //RIGHT REAR STEP FORWARDS
      for (counter = 1; counter <= 10; counter += 1) {
        pos8 = pos8 - 1;
        myservo8.write(pos8);
        delay(i);
      }
      for (counter = 1; counter <= 30; counter += 1) {
        pos7 = pos7 - 1;
        myservo7.write(pos7);
        delay(i);
      }
      for (counter = 1; counter <= 19; counter += 1) {
        pos8 = pos8 + 1;
        myservo8.write(pos8);
        delay(i);
      }
      for (counter = 1; counter <= 28; counter += 1) {
        pos7 = pos7 + 1;
        myservo7.write(pos7);
        delay(i);
      }
      for (counter = 1; counter <= 5; counter += 1) {
        pos7 = pos7 - 1;
        myservo7.write(pos7);
        delay(i);
      }

      //NEUTRAL POSITION
      pos1 = 65;
      pos2 = 80;
      pos3 = 120;
      pos4 = 80;
      pos5 = 42;
      pos6 = 95;
      pos7 = 128;
      pos8 = 75;
      myservo1.write(pos1);
      myservo2.write(pos2);
      myservo3.write(pos3);
      myservo4.write(pos4);
      myservo5.write(pos5);
      myservo6.write(pos6);
      myservo7.write(pos7);
      myservo8.write(pos8);
      delay(300);
    }
    else {
      delay(500);
    }
  }
}