However, when i add an if condition to the code like below the motors are getting to slow i mean totally to slow
if(sonar.ping_cm() >8 )// using NewPing library
{
digitalWrite(LM_Dir,LOW);
digitalWrite(RM_Dir,HIGH);
digitalWrite(LM_Step,HIGH);
digitalWrite(RM_Step,HIGH);
delay(1);
digitalWrite(LM_Step,LOW);
digitalWrite(RM_Step,LOW);
delay(1);
}
else digitalWrite(stopMotor,LOW);// this pin is high from setup() and connect to digital pin in arduino and to the 5v logic in driver
The ultrasonic sensor code is slowing the Stepper motor. When you use this line of code : duration = pulseIn(echoPin, HIGH); the pulseIn function makes the compiler wait for the incoming ultrasonic wave which in turn adds delay in the stepper motor routine.
Consider switching to a delay()-less and pulseIn-less solution using millis() & micros() and a pin interrupt.
(Compiles but not tested. Pin numbers will have to change. You only showed a snippet so fill in whatever else you need yourself...)
#define DIST_MEAS_PERIOD 100000 //micros() counts; 100mS intervals for ultrasound read
#define ECHO_TIMEOUT 5831 //micros() counts (5831 * 0.0343 / 2 == object 1m away; any further and we ignore)
#define STEP_INTERVAL 1000 //micros() counts (1 millisecond)
volatile bool
bEchoReceived;
float
fDistance;
unsigned long
tPulseDuration;
const byte LM_Dir = 2; //change pins for your own use
const byte RM_Dir = 2;
const byte LM_Step = 2; //change pins for your own use
const byte RM_Step = 2;
const byte trigPin = 2;
const byte echoPin = 2; //must be available for external interrupt
const byte stopMotor = 2;
void setup()
{
pinMode( LM_Step, OUTPUT );
pinMode( RM_Step, OUTPUT );
digitalWrite( LM_Dir,LOW );
digitalWrite( RM_Dir,LOW );
//
pinMode( LM_Dir, OUTPUT );
pinMode( RM_Dir, OUTPUT );
pinMode( stopMotor, OUTPUT );
digitalWrite( LM_Dir,LOW );
digitalWrite( RM_Dir,HIGH );
digitalWrite( stopMotor, LOW );
//
pinMode( trigPin, OUTPUT );
digitalWrite( trigPin, LOW );
pinMode( echoPin, INPUT ); //ultrasonic expected to assert this pin high; no pullup needed
bEchoReceived = false;
fDistance = 0.0;
}//setup
void loop()
{
UltraSoundSM();
StepperControl();
}//loop
//state names
#define ST_PING 0
#define ST_WAIT_ECHO 1
//
void UltraSoundSM( void )
{
static unsigned long
timeUSPeriod = 0,
timeUS;
static byte
stateUS = ST_PING;
unsigned long
timeNow;
timeNow = micros();
switch( stateUS )
{
case ST_PING:
//is it time for a measurement?
if( (timeNow - timeUSPeriod) < DIST_MEAS_PERIOD )
return; //no, just return
//setup for next measurement time
timeUSPeriod = timeNow;
//pulse the trigger pin
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
//get the time now (for measuring timeout period)
timeUS = micros();
//ISR is inactive at this point so should need to frame with
//interrupt-disable but good practice
noInterrupts();
bEchoReceived = false;
interrupts();
//turn on the pin interrupt for the echo
attachInterrupt(digitalPinToInterrupt(echoPin), ISR_Echo, CHANGE);
//move to wait-for-echo state
stateUS = ST_WAIT_ECHO;
break;
case ST_WAIT_ECHO:
//have we timed out waiting for an echo?
if( (timeNow - timeUS) > ECHO_TIMEOUT )
{
//yes; turn off the interrupt, and
detachInterrupt( digitalPinToInterrupt(echoPin) );
//set the distance to 0.0, and
fDistance = 0.0;
//go back to ping state
stateUS = ST_PING;
}//if
else if( bEchoReceived )
{
//when ISR sees rising and falling edges, it sets bEchoReceived
//tPulseDuration should have pulse width in microseconds; compute distance
fDistance = (float)tPulseDuration * 0.0343/2.0;
//and go to ping state
stateUS = ST_PING;
}//else
break;
}//switch
}//UltraSoundSM
void ISR_Echo( void )
{
static unsigned long
tPulseStart;
static bool
bPulseEdge = true;
//on this interrupt, is the pin high?
if( digitalRead( echoPin ) == HIGH )
{
//yes; get the micros count now and leave
tPulseStart = micros();
}//if
else
{
//pin is low marking end of pulse
//compute the pulse width in microseconds
tPulseDuration = micros() - tPulseStart;
//turn off the pin interrupts
detachInterrupt( digitalPinToInterrupt(echoPin) );
//and flag the state machine that a pulse was received
bEchoReceived = true;
}//else
}//ISR_Echo
void StepperControl( void )
{
static bool
bStepFlag = true;
static unsigned long
timeStep;
unsigned long
timeNow;
///if distance shows more than 8cm (0.0 shows no echo at all...)
if( fDistance < 0.1 || fDistance > 8 )
{
//are allowed to step motor if distances ok
digitalWrite( stopMotor,HIGH );
//check if time to step
timeNow = micros();
if( (timeNow - timeStep) > STEP_INTERVAL )
{
//set step pins to reflect state of bStepFlag
digitalWrite( LM_Step, (bStepFlag==true)?HIGH:LOW);
digitalWrite( RM_Step, (bStepFlag==true)?HIGH:LOW);
//now toggle bStepFlag using an exclusive-OR
bStepFlag ^= true;
//and set to time next step
timeStep = timeNow;
}//if
}//if
else
digitalWrite(stopMotor,LOW); //stop motors if US sees something close
}//StepperControl
TheNoneP:
thanks for that.
the code from Blackfin is good in term of reducing the blocking but how if i have multiple ultrasonics ?
The code can be modified to work with multiple sensors if you time-division multiplex them. Their pins can be set-up in arrays and indexed using common logic/code.
BTW, there's no need to do a check of the sensor(s) every loop() which happens on a microsecond time-scale. I think I have it set to 10Hz (100mS) and even that is pretty rapid.