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; }