long MeUltrasonicSensor::measure(){
	long duration;
	pinMode(s2, OUTPUT);
	MeOutput::write1(0);
	delayMicroseconds(2);
	MeOutput::write1(1023);
	delayMicroseconds(10);
	MeOutput::write1(0);
	pinMode(s1,INPUT);
	duration = pulseIn(s1,HIGH);
	return duration;
}

long Ultrasonic::getTimming()
{
	digitalWrite(trigger_pin, LOW); 
	delayMicroseconds(2); 

	digitalWrite(trigger_pin, HIGH);
	delayMicroseconds(10); 

	digitalWrite(trigger_pin, LOW);

	return pulseIn(echo_pin, HIGH, timeout);
}

///////////////////////
// Internal functions
///////////////////////
int Distance::checkModule(uint8_t module)
{
  long duration, distance;
  startPwm();
  setShift(module, PULSE_TIME);
  duration = pulseIn(ULTRASOUND_RECEIVE_PIN, HIGH);
  distance = (duration * CM_PER_MICROSECOND) + FIXED_OFFSET;
  //distance = (.13077*pow(duration,1-.2421));
  stopPwm();
  delay(250);
  return (int)distance;
}

long Ultrasonic::Timing()
{
  digitalWrite(Trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig_pin, LOW);
  duration = pulseIn(Echo_pin,HIGH,Time_out);
  if ( duration == 0 ) {
	duration = Time_out; }
  return duration;
}

ultrasonic::udistance()
{
    
       digitalWrite(trigPin,HIGH);
       delayMicroseconds(15);
       digitalWrite(trigPin,LOW);
       delayMicroseconds(2);
       duration = pulseIn(echoPin, HIGH);
       uldistance = duration/58.2;
       return uldistance;
    
}

// HC-SR04 ultrasonic sensor driver
unsigned int SonarControl::readHCSR04(int triggerPin, int echoPin){
  unsigned int uS;  
  digitalWrite(triggerPin, LOW); 
  delayMicroseconds(2); 
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(triggerPin, LOW);
  uS = pulseIn(echoPin, HIGH, MAX_ECHO_TIME + 1000);  
  if (uS > MAX_ECHO_TIME) uS = NO_ECHO;
    else if (uS < MIN_ECHO_TIME) uS = NO_ECHO;
  return uS;
}

void SonarTask(void* p)
{

	float cmdistance_1; //left
	float cmdistance_2; //right
	long microsec = ultrasonic.timing(); //MODIFIED DELAY FUNCTION
	long microsec_2 = ultrasonic_2.timing();
	
	    digitalWrite(trigpin_1, LOW);
		digitalWrite(trigpin_2, LOW);
	    vTaskDelay(2);
	    digitalWrite(trigpin_1, HIGH);
		digitalWrite(trigpin_2, HIGH);
	    vTaskDelay(10);
	    digitalWrite(trigpin_1, LOW);
		digitalWrite(trigpin_2, LOW);
	    long microsec_1 = pulseIn(echopin_1, HIGH);
		long microsec_2 = pulseIn(echopin_2, HIGH);
	    cmdistance_1 = microsec_1 / cmdivisor / 2.0;
		cmdistance_2 = microsec_2 / cmdivisor / 2.0;
		
	    int distance_left = (int) cmdistance_1;
		int distance_right = (int) cmdistance_2;
		
		if(cmdistance>40 && cmdistance<60) {

		}
		else if (cmdistance<40)	{

		}
		else { }

		if(cmdistance>40 && cmdistance<60) {

		}
		else if (cmdistance<40)	{

		}
		else { }
}

DHT11Result DHT11::read()
{
  pinMode(_pin, OUTPUT);
  digitalWrite(_pin, LOW);
  delay(20);

  uint32_t cycles[80];

  noInterrupts();
  {
    pinMode(_pin, INPUT_PULLUP);

    if (pulseIn(_pin, HIGH) == 0) {
      return prepareErrorResult("Timeout waiting for start signal LOW pulse");
    }

    for (int i = 0; i < 80; i+= 2) {
      cycles[i] = pulse(LOW, 150);
      cycles[i + 1] = pulse(HIGH, 150);
    }

    interrupts();
  }

  uint32_t data[5];
  data[0] = data[1] = data[2] = data[3] = data[4] = 0;

  for (int i = 0; i < 40; ++i) {
    uint32_t lowCycles  = cycles[2 * i];
    uint32_t highCycles = cycles[2 * i + 1];

    data[i/8] <<= 1;

    if (highCycles > lowCycles) {
      data[i/8] |= 1;
    }
  }

  uint32_t calculatedParity = (data[0] + data[1] + data[2] + data[3]) & 0xFF;

  if (data[4] != calculatedParity)
  {
    return prepareErrorResult("Parity check failed");
  }

  DHT11Result result;
  result.hasError = false;
  result.errorMessage = "";
  result.humidity = data[0];
  result.temperatureC = data[2];
  return result;
}

void loop() {
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
	long duration, inches, cm;

// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
	pinMode(pingPin, OUTPUT);
	digitalWrite(pingPin, LOW);
	delayMicroseconds(2);
	digitalWrite(pingPin, HIGH);
	delayMicroseconds(10);
	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(inPin, INPUT);
	duration = pulseIn(inPin, HIGH);

// convert the time into a distance
	inches = microsecondsToInches(duration);
	cm = microsecondsToCentimeters(duration);

	Serial.println(cm, DEC);

	if (cm < 3) {
		// play the note corresponding to this sensor:
		tone(8, notes[4], 30);
	}

	if ((cm >= 3) && (cm <= 9)) {
		// play the note corresponding to this sensor:
		tone(8, notes[0], 30);
	}

	if ((cm >= 10) && (cm <= 19)) {
		// play the note corresponding to this sensor:
		tone(8, notes[1], 30);
	}
	if ((cm >= 20) && (cm <= 29)) {
		// play the note corresponding to this sensor:
		tone(8, notes[2], 30);
	}

	if ((cm >= 30) && (cm <= 49)) {
		// play the note corresponding to this sensor:
		tone(8, notes[3], 30);
	}

	delay(100);
}

void sonar_loop()
{
  pinMode(pwPin, INPUT);
  pulse = pulseIn(pwPin, HIGH);
  inches = pulse / 147;
  cm = inches * 2.54;
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  delay(500);
}

float BlockingRC::getValueWithDelay(int pinIndex, long int delayInMicros)   //get the current RC reading for the channel 'pinIndex', provided that the time elapsed since the last reading was taken is greater than delayInMicros
{
	long int currentTime = micros();
	if((currentTime - lastReadTime)>delayInMicros)
	{
	lastReadTime = currentTime;
	return lastValue = pulseIn(pin[pinIndex], HIGH, 25000);
	}
	else
	{
	return lastValue;
	}
}

void frontleftsensor(){
  //———Front Left Sensor-------- 
  digitalWrite(trigPinFL, HIGH); //triggers sonar
  delayMicroseconds(1000);
  digitalWrite(trigPinFL, LOW); //turn off the sonar sender
  durationFL = pulseIn(echoPinFL, HIGH);//measures distance
  distanceFL = (durationFL/2) / 29.1; //converts to cm
 //Serial.print("distanceFL = ");
 //Serial.println(distanceFL);
  if (distanceFL<=10)
    frontleftsensor();
  delay(5);
}

unsigned int Remote::measure(byte channel)
{
	unsigned int measurement;

	measurement = pulseIn(_pins[channel], HIGH, 6000); // 30000 for servo at 50Hz, 5500 at 300Hz
	
	if(_debug)
	{
		printUnsigned("Remote Measure: ", measurement);
	}

	return(measurement); 
}

unsigned long LUltrasonic::measureDistance() {
	unsigned long duration, distance; 

	digitalWrite(_triggerPin, LOW); 
	delayMicroseconds(10); 
	digitalWrite(_triggerPin, HIGH);
	delayMicroseconds(20);
	digitalWrite(_triggerPin, LOW);
	duration = pulseIn(_echoPin, HIGH);
	distance = duration / 29 / 2;
    
    return distance <= MAX_MEASUREABLE_DISTANCE ? distance : INFINITE_DISTANCE;
}

long Me_UltrasonicSensor::measure()
{
	long duration;
	pinMode(signalPin, OUTPUT);
	digitalWrite(signalPin, LOW);
	delayMicroseconds(2);
	digitalWrite(signalPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(signalPin,LOW);
	pinMode(signalPin,INPUT);
	duration = pulseIn(signalPin,HIGH);
	return duration;
}

int get_distance(){
  //returns distance in centimeters from a ping sensor setup on pin 53
  pinMode(53, OUTPUT);
  digitalWrite(53, LOW);
  delayMicroseconds(2);
  digitalWrite(53, HIGH);
  delayMicroseconds(5);
  digitalWrite(53, LOW);

  pinMode(53, INPUT);

  return pulseIn(53, HIGH) / 29 / 2;
}

/**
 * \par Function
 *   measure
 * \par Description
 *   To get the duration of the ultrasonic sensor
 * \param[in]
 *   timeout - This value is used to define the measurement range, The
 *   default value is 30000.
 * \par Output
 *   None
 * \return
 *   The duration value associated with distance
 * \par Others
 *   None
 */
long MeUltrasonicSensor::measure(unsigned long timeout)
{
  long duration;
  MePort::dWrite2(LOW);
  delayMicroseconds(2);
  MePort::dWrite2(HIGH);
  delayMicroseconds(10);
  MePort::dWrite2(LOW);
  pinMode(s2, INPUT);
  duration = pulseIn(s2, HIGH, timeout);
  delayMicroseconds(200);
  return(duration);
}

    long pulse(int mode, double timeout = 5) {
        if(pin == NULL) {
            puts("Pin not initialized!");
            return 0;
        }
        if(this->mode == OUTPUT){
            //muxPin(pin, 0x27);
            digitalMode(pin, INPUT);
            this->mode = INPUT;
        }

        return pulseIn(pin, mode, timeout);
    }

int UltrasonicSensor::getDistance(){
	pinMode(trig, OUTPUT);
	digitalWrite(trig,LOW);
	delayMicroseconds(2);
	digitalWrite(trig, HIGH);
	delayMicroseconds(5);
	digitalWrite(trig, LOW);
	
	pinMode(echo, INPUT);
	int duration = pulseIn(echo, HIGH,11800);
	int distance = duration/29/2;
	
	return distance;
}

bool LeWeiPPDSensor::getValue(double *i)
{
    const unsigned long sampletime_ms = 30000;
    unsigned long lowpulseoccupancy = 0;

    unsigned long stoptime = millis() + sampletime_ms;
    while (((signed long)millis() - (signed long)stoptime) < 0)
        lowpulseoccupancy += pulseIn(_pin, LOW);

    float ratio = lowpulseoccupancy/(sampletime_ms*10.0);  // Integer percentage 0=>100
    *i = 1.1*pow(ratio,3)-3.8*pow(ratio,2)+520*ratio+0.62; // using spec sheet curve

    return true;
}