//------------------------------------------------------------------------
// Routine: TriggerEcho(TriggerPin,EchoPin) - 
// Triggers a Pulse and wait for an Echo.
//------------------------------------------------------------------------
float TiggerEcho(int TriggerPin, int EchoPin)
{
	long mPwidth = 0;    			//Pulse Width
	// 
	digitalWrite(TriggerPin, HIGH); //Trigger a 10us duration sonic burst.
	delayMicroseconds(10);
	digitalWrite(TriggerPin, LOW);
	//
	mPwidth = pulseIn(EchoPin, HIGH);
	//
	return mPwidth;    //Return Pulse Width measure to calling program.
}

						

//------------------------------------------------------------------------
//  Proximity Sensor SR-04
//  Routine to read an SR-04 Sensors using a Mega 2560 Controller.
//  Uses three leds to help debug the program.
//  July 3, 2018
//------------------------------------------------------------------------
#include <stdio.h>
//
//Memory variables
//
//Assigning Name to Controller Board
String mCntrlrName = "A1";
float mcm_L;
float mpWidth;
//
bool mStartOfLoop = true;        //Green LED
bool mReadCommPort = false;        //Blue LED
bool mReadingSensors = false;    //Red LED
bool mPrintCommPort = false;    //Yellow LED
bool mEndOfLoop = false;        //Green LED
//
//Implement Distance MIN and MAX in cm and inches
const unsigned int cMAX_DISTANCEcm = 400;    //Assume this number if object farther than certain distance. 400cm.
const unsigned int cMIN_DISTANCEcm = 30;    //Sound the buzzer if object too close. 50cm.
//
//Variables to talk to the Master Computer via USB port
byte mcmdByte0;
byte mcmdByte1;
byte mcmdByte2;
byte mcmdByte3;
//
//Declaration of pins to be used
//
//Proximity Sensors
const int pTrigger_Pin_L = 7;     //Left Sensor
const int pEcho_Pin_L = 6;
//
//Leds
const int pLedPin31 = 31;         //Start of Loop.
const int pLedPin32 = 32;         //Reading Communications Port.
const int pLedPin33 = 33;         //Reading Proximity Sensor.
const int pLedPin34 = 34;         //Printing To Communications Port.
const int pLedPin35 = 35;         //End of Loop and cycling back to start of loop.
//
//Buzzer
const int pBuzzerPin = 13;
//
//------------------------------------------------------------------------
// Routine: setup() - 
// Main Setup.
//------------------------------------------------------------------------
void setup() 
{
	pinMode(pLedPin31,OUTPUT);      //Start of Loop.
	pinMode(pLedPin32,OUTPUT);      //Reading Communications Port.
	pinMode(pLedPin33,OUTPUT);      //Trigger and get Echo from Sensor.
	pinMode(pLedPin34,OUTPUT);      //Print measure to computer via USB.
	pinMode(pLedPin35,OUTPUT);      //Print measure to computer via USB.
	digitalWrite(pLedPin31,HIGH);   //GREEN
	digitalWrite(pLedPin32,LOW);    //BLUE
	digitalWrite(pLedPin33,LOW);    //RED
	digitalWrite(pLedPin34,LOW);    //YELLOW
	digitalWrite(pLedPin35,LOW);    //GREEN
	//
	pinMode(pBuzzerPin,OUTPUT);    //initialize the buzzer pin as an output
	//
	pinMode(pTrigger_Pin_L, OUTPUT);
	digitalWrite(pTrigger_Pin_L, LOW);
	//
	pinMode(pEcho_Pin_L, INPUT);
	//
	// Activate serial monitor to send/receive communications using 9600 bauds
	Serial.begin(9600);
}
//
//------------------------------------------------------------------------
// Routine: loop() - 
// Main Loop.
//------------------------------------------------------------------------
void loop()
{
	mStartOfLoop = !mStartOfLoop;
	digitalWrite(pLedPin31, mStartOfLoop);  //GREEN LED on/off
	//
	if (Serial.available() > 2)
	{
		mReadCommPort = !mReadCommPort;
		digitalWrite(pLedPin32, mReadCommPort); //BLUE LED on/off
		//
		mcmdByte0 = Serial.read();    delay(10);
		mcmdByte1 = Serial.read();    delay(10);
		mcmdByte2 = Serial.read();    delay(10);
		mcmdByte3 = Serial.read();    delay(10);
	}
	// 
	if (mcmdByte0 == 1)          // First byte is equal to "1", then someone maybe talking to Arduino,
	{
	if (mcmdByte1 == 1)        // Second byte is equal to "1".
	{
		if (mcmdByte2 == 1)      // Third byte is equal to "1".
		{
			if (mcmdByte3 == 1)    // Forth byte is equal to "1", tell me your ID!
			{
				Serial.print(mCntrlrName);
			}
		}
	}
	//
	//..
	if (mcmdByte1 == 2) {}
	if (mcmdByte1 == 3) {}
	//..
	//
	if (mcmdByte1 == 4)
	{
		if (mcmdByte2 == 7)
		{
			PollSensors();    //Read Proximity Sensors.
		}
		if (mcmdByte2 == 8) {}
		if (mcmdByte2 == 9) {}
	}
	}
	//
	mcmdByte0 = 0;    //Reset Comm bytes to zero.
	mcmdByte1 = 0;
	mcmdByte2 = 0;
	mcmdByte3 = 0;
	//
	mEndOfLoop = !mEndOfLoop;
	digitalWrite(pLedPin35, mEndOfLoop);  //Green LED on/off
}
//
//------------------------------------------------------------------------
// Routine: PollSensors() - 
// Poll then Sensors.
//------------------------------------------------------------------------
void PollSensors()
{
	ReadSensors();    //Call ReadSensors Routine
	//
	PrintMeasures();    //Call PrintMeasures Routine
	//
	delay(100);        //Wait a few millisecs before loop for next measurement
}
//
//------------------------------------------------------------------------
// Routine: ReadSensors() - 
// Reads all the sensors.
//------------------------------------------------------------------------
void ReadSensors()
{
	mReadingSensors = !mReadingSensors;        //Red LED on/off
	digitalWrite(pLedPin33,mReadingSensors);
	//------------------------------------------------------------------------
	mpWidth = TiggerEcho(pTrigger_Pin_L, pEcho_Pin_L);
	mcm_L = mpWidth / 58.0;        //Convert the pulse width to centimeters
							//That is 58us/cm. or 148us/in.
	//------------------------------------------------------------------------
	//Too close so play the buzzer.
	if (mcm_L <= cMIN_DISTANCEcm){    buzzerX(); }
}
//
//------------------------------------------------------------------------
// Routine: TriggerEcho(TriggerPin,EchoPin) - 
// Triggers a Pulse and wait for an Echo.
//------------------------------------------------------------------------
float TiggerEcho(int TriggerPin, int EchoPin)
{
	long mPwidth = 0;    //Pulse Width
	// 
	digitalWrite(TriggerPin, HIGH);    //Trigger a 10us duration sonic burst.
	delayMicroseconds(10);
	digitalWrite(TriggerPin, LOW);
	//
	mPwidth = pulseIn(EchoPin, HIGH);
	//
	return mPwidth;    //Return Pulse Width measure to calling program.
}
//
//------------------------------------------------------------------------
// Routine: PrintMeasures() - 
// Print Measures.
//------------------------------------------------------------------------
void PrintMeasures()
{
	mPrintCommPort = !mPrintCommPort;        //Yellow LED on/off
	digitalWrite(pLedPin34,mPrintCommPort);
	//------------------------------------------------------------------------    
	String mL((float)mcm_L);
	Serial.print("L"+mL+"cm ");
}
//
//-------------------------------------------------------------------------------
// buzzerX Routine - Sound the buzzer.
//-------------------------------------------------------------------------------
void buzzerX()
{
	for(int i=0;i < 5;i++)
	{
		digitalWrite(pBuzzerPin,HIGH);    //Turn buzzer on
		delay(5);                        //wait 5ms
		digitalWrite(pBuzzerPin, LOW);    //Turn buzzer off
	}
}