void Servotor32::delay_us(long unsigned int delay_time){
startTime = micros_new();
currentTime = micros_new() - startTime;
while(currentTime < delay_time){
delayMicroseconds(10);
currentTime = micros_new() - startTime;
}
}
float Servotor32::ping(){
//PB0 for Trigger (17)
//PB7 for Echo (11)
pinMode(17,OUTPUT);
pinMode(11,INPUT);
long duration;
float cm;
digitalWrite(17, LOW);
delayMicroseconds(2);
digitalWrite(17, HIGH);
delayMicroseconds(5);
digitalWrite(17, LOW);
// duration = pulseIn(11, HIGH, 100000);
uint8_t bit = digitalPinToBitMask(11);
uint8_t port = digitalPinToPort(11);
uint8_t stateMask = (HIGH ? bit : 0);
unsigned long startCount = 0;
unsigned long endCount = 0;
unsigned long width = 0; // keep initialization out of time critical area
// convert the timeout from microseconds to a number of times through
// the initial loop; it takes 16 clock cycles per iteration.
unsigned long numloops = 0;
unsigned long maxloops = 500;
// wait for any previous pulse to end
while ((*portInputRegister(port) & bit) == stateMask)
if (numloops++ == maxloops)
return 0;
// wait for the pulse to start
while ((*portInputRegister(port) & bit) != stateMask)
if (numloops++ == maxloops)
return 0;
startCount = micros_new();
// wait for the pulse to stop
while ((*portInputRegister(port) & bit) == stateMask) {
if (numloops++ == maxloops)
return 0;
delayMicroseconds(10); //loop 'jams' without this
if((micros_new() - startCount) > 58000 ){ // 58000 = 1000CM
return 0;
break;
}
}
duration = micros_new() - startCount;
//--------- end pulsein
cm = (float)duration / 29.0 / 2.0;
return cm;
}
