Robotic Avoidance Car
Avoidance Car Sketch
//**********************************************************************
// Avoidance Car
// ServoDir Pin 12
// Trig Pin 8
// Echo Pin 9
// L298 LFT Motor IN1(D1),IN2(D0), Rt Motor IN3(D4),IN4(D2) DIGITAL PINS
// L298 LFTENA-D3, RTENA-D11 PWM PINS
// CG90 MOTOR: 1ms pls = LEFT, 1.5 ms pulse is middle"90", 2ms pls = Right
//**********************************************************************
#include
//********************* DEFINES ****************************************
#define RT_FWD 1 // DEFINE 5 PIN
#define RT_BAK 0 // DEFINE 4 PIN
#define LFT_FWD 4 // DEFINE 3 PIN
#define LFT_BAK 2 // DEFINE 2 PIN
#define R_ENA 3 // DEFINE 1 PIN (PWM)
#define L_ENA 11 // DEFINE 0 PIN (PWM)
#define ECHO_PIN 9 // DEFINE ULTRASOUND ECHO PIN
#define TRIG_PIN 8 // DEFINE ULTRASOUND TRIGGER PIN
#define FWD 1 // FORWARD
#define RT 2 // TURN RIGHT
#define LFT 3 // TURN LEFT
#define BAK 4 // BACKWARD
#define LFTDIR 177 // LEFT DIR
#define FWDDIR 90 // FORWARD DIR
#define RTDIR 3 // RIGHT DIR
#define FAST 200 // SPEED FAST
#define SLOW 100 // SPEED SLOW
#define STOP 0 // SPEED STOP
#define SrvoPin 12 // SERVO DIRECTION
//********************* VARIABLES ****************************************
Servo SnsrDir; // PING DIRECTION
int OldDir, OldTurn;
int DistL;
int DistR;
int DistF;
int CurDir;
//********************* PROTOTYPES ****************************************
void mvFwd( int dly, int spd );
void turnR( int dly );
void turnL( int dly );
void backUp( int dly );
void stop();
int pickDir();
int look( int dir );
/*F**********************************************************************
*
**********************************************************************/
void
setup()
{
pinMode( LFT_BAK, OUTPUT ); // DEFINE LEFT BACK OUTPUT (PWM)
pinMode( LFT_FWD, OUTPUT ); // DEFINE LEFT FWD OUTPUT (PWM)
pinMode( RT_BAK, OUTPUT ); // DEFINE RIGHT BACK OUTPUT (PWM)
pinMode( RT_FWD, OUTPUT ); // DEFINE RIGHT FWD OUTPUT (PWM)
pinMode( L_ENA, OUTPUT ); // DEFINE LEFT MOT ENA (PWM)
pinMode( R_ENA, OUTPUT ); // DEFINE RIGHT MOT ENA (PWM)
pinMode( ECHO_PIN, INPUT ); // DEFINE ULTRASOUND ECHO PIN
pinMode( TRIG_PIN, OUTPUT); // DEFINE ULTRASOUND TRIGGER PIN
SnsrDir.attach( SrvoPin ); // DEFINE SENSOR DIRECTION SERVO
digitalWrite( TRIG_PIN, LOW); // INIT TRIGGER PIN LO
}
/*F**********************************************************************
*
**********************************************************************/
void
loop()
{
int dir; // FWDDIR, RTDIR, or LFTDIR
SnsrDir.write( FWDDIR ); // MOVE SENSOR TO CENTER
dir = pickDir(); // MEASURE DIST & DETERMINE WHICH DIR TO GO
switch( dir )
{
case BAK:
// LOOK BAK HERE
if( OldDir != BAK )
stop();
backUp( 1 ); // BACK
turnL( 2 );// MOVE SLIGHTLY LEFT (PREVENT STUCK IN DEAD END LANE)
break;
case RTDIR:
if( OldDir != RTDIR )
stop();
backUp( 1 );
turnR( 2 ); // TURN RIGHT
break;
case LFTDIR:
if( OldDir != LFTDIR )
stop();
backUp( 1 );
turnL( 2 );
break;
case FWDDIR:
// if( DistF > 30 )
// mvFwd( 1, FAST ); // DIR == FWD
// else
mvFwd( 1, SLOW ); // DIR == FWD
break;
} // END SWITCH dir
OldDir = dir;
}
/*F**********************************************************************
* MEASURE DIST AT DIRECTION ANGLE
**********************************************************************/
int
look( int dir )
{
float time;
int dist;
digitalWrite( TRIG_PIN, LOW); // INIT TRIGGER PIN LO
SnsrDir.write( dir ); // MOVED SENSOR TO DIR
if( CurDir != FWDDIR )
delay( 400 ); // WAIT FOR SENSOR TO MOVE & STABILIZE
else
delay( 200 ); // WAIT FOR SENSOR TO MOVE & STABILIZE
digitalWrite( TRIG_PIN, HIGH); // SET TRIG PIN HI
delayMicroseconds( 10 ); // WAIT 10US
digitalWrite( TRIG_PIN, LOW); // SET TRIG PIN LO
time = pulseIn( ECHO_PIN, HIGH ); // RD TIME DIFF IN US
dist = time / 148; // CONVERT TIME TO DIST (IN)
CurDir = dir;
return( dist ); // RETURN CONVERTED DISTANCE
}
/*F**********************************************************************
* MEASURE ALL DISTANCES AND DETERMINE WHICH DIR TO GO
**********************************************************************/
int
pickDir()
{
int dir = FWD, dlyTim = 250;
DistF = look( FWDDIR ); // READ DISTANCE FORWARD
if( DistF < 12) // IF FORWARD DIST LESS THAN 12 IN
{
stop();
return( BAK );
}
if( DistF < 20) // IF FRONT DISTANCE LESS THAN 20 IN
{
stop(); // STOP MOVING
DistL = look( LFTDIR ); // READ LEFT DIST
delay( dlyTim ); // WAIT FOR SERVO POS TO STABLIZE
DistR = look( RTDIR ); // READ RIGHT DIST
delay( dlyTim ); // WAIT FOR SERVO POS TO STABLIZE
if( DistL > DistR ) // IF LFT DIST > RIGHT DIST
dir = LFTDIR; // GO LEFT
if( DistL <= DistR) //IF LFT DIST <= RIGHT DIST
dir = RTDIR; // GO RIGHT
if( DistL < 8 && DistR < 8 ) // IF LEFT DIST AND RIGHT DIST < 8 IN
dir = BAK; // GO BACKWARD
}
else // IF FRONT >= 12 IN
dir = FWDDIR; // MOVE FORWARD
SnsrDir.write( FWDDIR ); // RETURN SENSOR FWD
return( dir );
}
/*F**********************************************************************
* GO BACKWARD
**********************************************************************/
void
backUp( int d )
{
// LOOK BACK B4 moving.
digitalWrite( RT_FWD, LOW ); // RIGHT MOTOR BACK
digitalWrite( RT_BAK, HIGH );
analogWrite( R_ENA, SLOW ); // RIGHT SPEED SLOW
digitalWrite( LFT_FWD, LOW ); // LEFT MOTOR BACK
digitalWrite( LFT_BAK, HIGH );
analogWrite( L_ENA, SLOW ); // LEFT SPEED SLOW
delay( d * 500);
}
/*F**********************************************************************
* GO FWD
**********************************************************************/
void
mvFwd( int dly, int spd )
{
digitalWrite( RT_FWD, HIGH ); // RIGHT MOTOR FWD
digitalWrite( RT_BAK, LOW );
analogWrite( R_ENA, spd ); // RIGHT SPEED
digitalWrite( LFT_FWD, HIGH ); // LEFT MOTOR FWD
digitalWrite( LFT_BAK, LOW );
analogWrite( L_ENA, spd ); // LEFT SPEED
delay( dly * 1 );
}
/*F**********************************************************************
* GO RIGHT
**********************************************************************/
void
turnR( int dly )
{
digitalWrite( RT_FWD, LOW); // RIGHT MOTOR BACK
digitalWrite( RT_BAK, HIGH);
analogWrite( R_ENA, SLOW); // RIGHT SPEED FAST
digitalWrite( LFT_FWD, HIGH); // LEFT MOTOR FWD
digitalWrite( LFT_BAK, LOW);
analogWrite( L_ENA, SLOW); // LEFT SPEED FAST
delay( dly * 50);
}
/*F**********************************************************************
* GO LEFT
**********************************************************************/
void
turnL( int dly )
{
digitalWrite( RT_FWD, HIGH ); // RIGHT MOTOR FWD
digitalWrite( RT_BAK, LOW );
analogWrite( R_ENA, SLOW ); // RIGHT SPEED SLOW
digitalWrite( LFT_FWD, LOW ); // LEFT MOTOR BACK
digitalWrite( LFT_BAK, HIGH );
analogWrite( L_ENA, SLOW ); // LEFT SPEED SLOW
delay( dly * 50);
}
/*F**********************************************************************
* STOP
**********************************************************************/
void
stop()
{
digitalWrite( RT_FWD, LOW); // RIGHT MOTOR STOP
digitalWrite( RT_BAK, LOW);
analogWrite( R_ENA, STOP ); // RIGHT SPEED ZERO
digitalWrite( LFT_FWD, LOW); // LEFT MOTOR STOP
digitalWrite( LFT_BAK, LOW);
analogWrite( L_ENA, STOP ); // LEFT SPEED ZERO
delay( 250);
}