Stepper + Sonar

#include <TimerOne.h>

#define TRIGGER_PIN 9 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 2 // 10 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 40 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

#define TIMER_US 10000                         // 10mS set timer duration in microseconds
#define TICK_COUNTS 20                          // 2S worth of timer ticks
#define STEPS 400

#define THROTTLE_SIGNAL_IN 0 // INTERRUPT 0 = DIGITAL PIN 2 - use the interrupt number in attachInterrupt
#define THROTTLE_SIGNAL_IN_PIN 2 // INTERRUPT 0 = DIGITAL PIN 2 - use the PIN number in digitalRead

#define NEUTRAL_THROTTLE 1500 // this is the duration in microseconds of neutral throttle on an electric RC Car



const int ledPin =  13;      // the number of the LED pin

unsigned long previousMic = 0;        // will store last time LED was updated
unsigned long currentMic;
unsigned int tmp,tmp2;

volatile long tick_count = TICK_COUNTS;         // Counter for 2S
volatile bool in_long_isr = false;              // True if in long interrupt
volatile bool timeout1 = false;

int SetDistance = 0;
int ValueDist = 0;
int motorPin1 = A1;
int motorPin2 = A2;
int motorPin3 = A3;
int motorPin4 = A4;
unsigned int uS;
unsigned int n_step;
int trig = TRIGGER_PIN;
int echo = ECHO_PIN;


unsigned int tempo;

volatile int nThrottleIn = NEUTRAL_THROTTLE; // volatile, we set this in the Interrupt and read it in loop so it must be declared volatile
volatile unsigned long ulStartPeriod = 0; // set in the interrupt
volatile boolean bNewThrottleSignal = false; // set in the interrupt and read in the loop
// we could use nThrottleIn = 0 in loop instead of a separate variable, but using bNewThrottleSignal to indicate we have a new signal
// is clearer for this first example


void setup() {
Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
Serial.println(" Hello " );

pinMode(A0,OUTPUT);
pinMode(A5,OUTPUT);
pinMode(A1,OUTPUT);
pinMode(A2,OUTPUT);
pinMode(A3,OUTPUT);
pinMode(A4,OUTPUT);


pinMode(trig,OUTPUT);
pinMode(echo,INPUT);


Timer1.initialize(TIMER_US);                  // Initialise timer 1
Timer1.attachInterrupt( timerIsr );           // attach the ISR routine here

digitalWrite(A0,HIGH);
digitalWrite(A5,HIGH);

attachInterrupt(THROTTLE_SIGNAL_IN,calcInput,CHANGE);
}

void loop() {

  while ( n_step++<100){

          digitalWrite(motorPin1, HIGH);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, LOW);

          while (tmp2++<12000);
          tmp2=0;
          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, HIGH);
          digitalWrite(motorPin4, LOW);

         while (tmp2++<12000);
          tmp2=0;
          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, HIGH);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, LOW);

          while (tmp2++<12000);
          tmp2=0;
          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, HIGH);
          while (tmp2++<12000);
          tmp2=0;
       }

        n_step = 0;

        while ( n_step++<100){
          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, HIGH);
          while (tmp2++<12000);
          tmp2=0;

          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, HIGH);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, LOW);
          while (tmp2++<12000);
          tmp2=0;
          digitalWrite(motorPin1, LOW);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, HIGH);
          digitalWrite(motorPin4, LOW);
          while (tmp2++<12000);
          tmp2=0;
          digitalWrite(motorPin1, HIGH);
          digitalWrite(motorPin2, LOW);
          digitalWrite(motorPin3, LOW);
          digitalWrite(motorPin4, LOW);
          while (tmp2++<12000);

        }

         n_step = 0;


}

// --------------------------
// timerIsr() 10 milli second interrupt ISR()
// Called every time the hardware timer 1 times out.
// --------------------------
void timerIsr()
{
if(bNewThrottleSignal)
 {
   Serial.println(nThrottleIn/58,DEC);   // misura in cm

   // set this back to false when we have finished
   // with nThrottleIn, while true, calcInput will not update
   // nThrottleIn
   bNewThrottleSignal = false;
 }

                digitalWrite(trig,HIGH);
                delayMicroseconds(10);
                digitalWrite(trig,LOW);
}


//Credits -> http://rcarduino.blogspot.it/2012/01/how-to-read-rc-receiver-with.html
void calcInput()
{
  // if the pin is high, its the start of an interrupt
  if(digitalRead(THROTTLE_SIGNAL_IN_PIN) == HIGH)
  {
    // get the time using micros - when our code gets really busy this will become inaccurate, but for the current application its
    // easy to understand and works very well
    ulStartPeriod = micros();
  }
  else
  {
    // if the pin is low, its the falling edge of the pulse so now we can calculate the pulse duration by subtracting the
    // start time ulStartPeriod from the current time returned by micros()
    if(ulStartPeriod && (bNewThrottleSignal == false))
    {
      nThrottleIn = (int)(micros() - ulStartPeriod);
      ulStartPeriod = 0;

      // tell loop we have a new signal on the throttle channel
      // we will not update nThrottleIn until loop sets
      // bNewThrottleSignal back to false
      bNewThrottleSignal = true;
    }
  }
}