Any chance you could add some comments to your code?
As well, it helps to name pins to give readers (including yourself in the future...) what pin is doing what.
const int pinTrigger_S1 = A1;
const int pinEcho_S1 = A2;
.
.
.
pinMode(pinTrigger_S1, OUTPUT);
pinMode(pinEcho_S1, INPUT);
.
.
.
digitalWrite( pinTrigger_S1, LOW );
delay( 200 );
digitalWrite( pinTrigger_S1, HIGH );
delayMicroseconds( 10 );
digitalWrite( pinTrigger_S1, LOW );
duration = pulseIn( pinEcho_S1, HIGH );
.
.
.
Also, can you clarify this: "The code works, but I do not want the motors to reverse for a 1 second obstruction so it should not reverse unless the distance of less than 10cm is true for a certain time period, say 10 seconds."
If the robot is moving"forward" and senses something, it should stop. Waiting 10 seconds will likely result in a collision (unless this robot is moving extremely slowly...)
How about something like this:
#define DISTANCE_CLEAR (float)25.0 //a value higher than the threshold for cases where no echo seen (pulseIn returns 0)
#define SENSOR_L 0b00000001
#define SENSOR_R 0b00000010
const int pinTrigger_S1 = A1;
const int pinEcho_S1 = A2;
const int pinTrigger_S2 = A3;
const int pinEcho_S2 = A4;
// Ardublockly generated sketch
float fDistance_S1;
float fDistance_S2;
byte DistanceFlags;
void setup()
{
Serial.begin(9600);
pinMode(pinTrigger_S1, OUTPUT);
pinMode(pinEcho_S1, INPUT);
pinMode(pinTrigger_S2, OUTPUT);
pinMode(pinEcho_S2, INPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
fDistance_S1 = -1.0; //initial value; negative means no measurement taken yet
fDistance_S2 = -1.0;
DistanceFlags = 0x00;
}//setup
#define CD_ST_0 0
#define CD_ST_1 1
#define CD_ST_2 2
#define DMSM_ST_0 0
#define DMSM_ST_1 1
#define DMSM_ST_2 2
#define DMSM_ST_3 3
#define DMSM_ST_4 4
#define DMSM_ST_5 5
bool CheckDistanceSensors( void )
{
static byte
stateCD = CD_ST_0;
static unsigned long
timeCD = millis();
unsigned long
timeNow;
switch( stateCD )
{
case CD_ST_0:
//100mS (10Hz) sensor updates
if( millis() - timeCD < 100 )
return;
digitalWrite(pinTrigger_S1, LOW);
digitalWrite(pinTrigger_S2, LOW);
timeCD = timeNow;
stateCD = CD_ST_1;
break;
case CD_ST_1:
timeNow = millis();
//200mS? that's a really long time...can it be shortened to, say, 5mS?
//the trigger output will have been low heading into this so...
if( (timeNow - timeCD) < 5 )
return;
timeCD = timeNow;
digitalWrite(pinTrigger_S1, HIGH);
delayMicroseconds(10);
digitalWrite(pinTrigger_S1, LOW);
//sound travels at about 0.0343cm/uS. 20cm (total time for threshold sensing
//returns about 1166 ticks. Put a timeout of 2500uS to avoid waiting too long
fDistance_S1 = (float)(pulseIn( pinEcho_S1, HIGH ), 2500ul ) / 58.309;
if( fDistance_S1 < 1 )
fDistance_S1 = DISTANCE_CLEAR;
Serial.print("Distance 1: "); Serial.println(fDistance_S1,1);
stateCD = CD_ST_2;
break;
case CD_ST_2:
timeNow = millis();
//sensor 2 measured 25mS after the first
if( (timeNow - timeCD) < 25 )
return;
digitalWrite(pinTrigger_S2, HIGH);
delayMicroseconds(10);
digitalWrite(pinTrigger_S2, LOW);
fDistance_S2 = (float)(pulseIn( pinEcho_S2, HIGH ), 2500ul ) / 58.309;
if( fDistance_S2 < 1 )
fDistance_S2 = DISTANCE_CLEAR;
Serial.print("Distance 2: "); Serial.println(fDistance_S2,1);
if( fDistance_S1 < 10 )
DistanceFlags |= SENSOR_L;
else
DistanceFlags &= ~SENSOR_L;
if( fDistance_S2 < 10 )
DistanceFlags |= SENSOR_R;
else
DistanceFlags &= ~SENSOR_R;
timeCD = timeNow;
stateCD = CD_ST_0;
//indicate new measurement is ready
return true;
break;
}//switch
//still processing, no new data yet
return false;
}//CheckDistanceSensors
void MotorsForward( byte spd )
{
digitalWrite( 7, HIGH );
digitalWrite( 8, LOW );
analogWrite( 6, spd );
digitalWrite( 9, HIGH );
digitalWrite( 10, LOW );
analogWrite( 11, spd );
}//MotorsForward
void MotorsReverse( byte spd )
{
digitalWrite( 9, LOW );
digitalWrite( 10, HIGH );
analogWrite( 11, spd );
digitalWrite( 7, LOW );
digitalWrite( 8, HIGH );
analogWrite( 6, spd );
}//MotorsReverse
void MotorsStop( void )
{
digitalWrite( 7, LOW );
digitalWrite( 8, LOW );
analogWrite( 6, 0 );
digitalWrite( 9, LOW );
digitalWrite( 10, LOW );
analogWrite( 11, 0 );
}//MotorsStop
void RevTurnRight( byte spd )
{
digitalWrite(7, LOW);
digitalWrite(8, LOW);
analogWrite(6, 0);
digitalWrite(9, HIGH);
digitalWrite(10, LOW);
analogWrite(11, spd); //slow turn
}//RevTurnRight
void DoMotorStateMachine( void )
{
static byte
stateDMSM = DMSM_ST_0;
unsigned long
timeNow;
static unsigned long
timeDMSM;
static byte
SpeedMotor1 = 0,
SpeedMotor2 = 0;
switch( stateDMSM )
{
case DMSM_ST_0:
//wait for initial sensor measurement so we know it's safe to proceed
if( fDistance_S1 < 0 || fDistance_S2 < 0 )
return;
stateDMSM = DMSM_ST_1;
break;
case DMSM_ST_1:
if( (DistanceFlags & SENSOR_R) || (DistanceFlags & SENSOR_L) )
{
//stop the robot, move to collision avoidance state (ST_2)
MotorsStop();
timeDMSM = millis();
stateDMSM = DMSM_ST_2;
}//if
else
{
//no obstruction seen, command "forward"
MotorsForward( 100 );
}//else
break;
case DMSM_ST_2:
if( (millis() - timeDMSM) < 1000 )
return;
timeDMSM = millis();
//start reversing slowly
//we'll monitoring the distance result during reversing and, if required, turning
//and so should only rev/turn as much as is needed
MotorsReverse( 50 );
stateDMSM = DMSM_ST_3;
break;
case DMSM_ST_3:
if( (millis() - timeDMSM) < 1000 )
return;
timeDMSM = millis();
//begin slow turn turn
RevTurnRight(50);
break;
case DMSM_ST_4:
if( DistanceFlags == 0 )
{
//we're clear of the obstruction
//stop turning and try going foward
MotorsStop();
stateDMSM = DMSM_ST_1;
}//if
else
{
//are still seeing obstruction
//if time expired, stop the motors and reverse some more
if( (millis() - timeDMSM) > 1000 )
{
MotorsStop();
timeDMSM = millis();
stateDMSM = DMSM_ST_2;
}//if
}//else
break;
}//switch
}//DoMotorStateMachine
void loop()
{
CheckDistanceSensors();
DoMotorStateMachine();
}//loop
(Compiles but not tested on a 2565. Made some assumptions. YMMV. Good luck...)
