e-Bot_4x4_Sumobot_with_Line_avoidance.ino

/*
   e-Gizmo eBOT 4x4 Sumobot with line avoidance
 (with US-100 sensor)
 
 Using State Machine library of Arduino.
 For eBOT sample code

 I/O Usage
  - collision sensor inputs (not used in this demo)
  2- colision1 as input
  3- colision2 as input
  4- colision3 as input

  - line sensor inputs
  5- linesense1 as input low on black
  6- linesense2 as input
  7- linesense3 as input

  - motor control output (default PBOT pin)
  8 - mot2 dir as output high=fwd (right motor)
  9 - mot2 run as output
  11 - mot1 dir as output high= fwd (left motor)
  10 - mot1 run as output

  - US-100 pin assignment
  12 - Trigger
   3 - Echo
 
  - Servo pin assignment
   7 - Signal Output
 
 Modified by Amoree
 @e-Gizmo Mechatronix Central
 Feb 2, 2016
 
 */
#include <NewPing.h>          // Runs the Ultrasonic Distance Sensor#include <SM.h>;
#include <SM.h>

#define  RampDelay  10

#define  StartMoveSpeed    200  // Motor Driver value for start of motion

#define  SlowMoveSpeed     280
#define  SlowMoveAdjust     -4   // Adjust for straight move: - Left + Right??

#define  MediumMoveSpeed   300
#define  MediumMoveAdjust  -8  // Adjust for straight move: - Left + Right

#define  FastMoveSpeed     350
#define  FastMoveAdjust    -8  // Adjust for straight move: - Left + Right

// Ultrasonic Sensor Pins
#define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     4  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
// Servo
#define SERVO_PIN    3  // The "Look Around" servo
#define PIN13_LED    13  // The onboard LED

//----( "TARGET FOUND" DIRECTIONS (index into TargetArray )---------
#define TARGET_FOUND_ANY     0   // Values will be "true" or "false"
#define TARGET_LEFT          1
#define TARGET_LEFT_CENTER   2
#define TARGET_CENTER        3
#define TARGET_RIGHT_CENTER  4
#define TARGET_RIGHT         5

#define TARGET_ARRAY_SIZE    6

#define TARGET_TOO_CLOSE     40

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Set pins and maximum distance.
SM RoboGo(RoboStartState);//create simple statemachine

/*-----( Declare Variables )-----*/
unsigned int uS;  // Result of a ping: MicroSeconds
unsigned int cm;  // Distance calculated for ping (0 = outside set distance range)
unsigned int cm_now; // For test

int TargetArray[TARGET_ARRAY_SIZE];  // Holds the directions a Target was found in
int DirectionsToLook = 3;  // For LookAround()
int ServoDirectionData[3] = { 
  2000, 1600, 900};

int runspeed=255;
long int rtc;
int  ls1 = 5;
int  ls2 = 6;
int  ls3 = 7;
int a = 0;
int b = 0;


void setup()   /******************* SETUP: RUNS ONCE *****************/
{
  Serial.begin(115200);
  delay(1000);
  Serial.println("Gizduino eBot Test");
  //--NOTE: Ultrasonic Sensor and Motor Pins set to OUTPUT by their libraries
  pinMode(8, OUTPUT);//M2DIR
  pinMode(9, OUTPUT);//M2RUN
  pinMode(10, OUTPUT);//M1RUN
  pinMode(11, OUTPUT);//M1DIR
  pinMode(SERVO_PIN,OUTPUT);  
  pinMode(PIN13_LED,OUTPUT);    
  rtc=millis()+10;
}//--(end setup )---

/************** LOOP: RUNS CONSTANTLY **************************/
void loop()   
{
  EXEC(RoboGo);//run statemachine
  delay(100);


  /*
  // Hardware Timer service
   if(millis()>=rtc)
   {
   rtc=rtc+10;
   if(retry_dly>0) retry_dly--;
   if(ledflsh>0)
   {
   ledflsh--;
   if(ledflsh==0)
   {
   ledflsh=25;
   PORTB ^= 0x20;
   }
   }                
   }      
   
   // read the status of colision and line sensors
   linesense=0;
   if(digitalRead(ls1)==LOW) linesense=1;
   if(digitalRead(ls2)==LOW) linesense=linesense+2;
   if(digitalRead(ls3)==LOW) linesense=linesense+4;
   
   
   // if no line is detected (all high)
   if((linesense==0) & (retry_dly==0))
   {
   if(giveup<10)
   {
   //reverse for 20mS
   if(lastsense==1) runBot(runspeed*15/10,runspeed,LOW);
   if(lastsense==3) runBot(runspeed*12/10,runspeed,LOW);
   if(lastsense==4) runBot(runspeed,runspeed*15/10,LOW);
   if(lastsense==6) runBot(runspeed,runspeed*12/10,LOW);
   
   delay(40);
   giveup++;
   }
   else
   Stop();
   //delay(1000);
   }
   
   // if line is detected
   if(linesense!=0)
   {
   lastsense=linesense;
   giveup=0;
   retry_dly=50;  // retry delay=500ms
   }        
   
   */
}//--(end main loop )---

int  linesense=0;
int giveup=0;
int lastsense;
//timers
byte  retry_dly=0;
byte  ledflsh=25;
/*----------------( Declare User-written Functions )---------------*/

//------( MOTOR CONTROL FUNCTIONS )----------------
void ForwardSlow()
{
  line_sense();
  digitalWrite(8, HIGH); 
  digitalWrite(9, 90);
  digitalWrite(10, 90);
  digitalWrite(11, HIGH);
}
/*---------------------------*/
void ForwardMedium()
{
  digitalWrite(8, HIGH); 
  digitalWrite(9, 200);
  digitalWrite(10, 200);
  digitalWrite(11, HIGH);
}
/*---------------------------*/
void ForwardFast()
{
  line_sense();
  digitalWrite(8, HIGH); 
  digitalWrite(9, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(11, HIGH);
}
/*---------------------------*/
void BackwardSlow(int HowMuch)
{
  digitalWrite(8, LOW); 
  digitalWrite(9, HowMuch);//run2
  digitalWrite(10, HowMuch);//run1
  digitalWrite(11, LOW);
  delay(1000);

}
/*---------------------------*/
void BackwardMedium(int HowMuch)
{
  digitalWrite(8, LOW); 
  digitalWrite(9, HowMuch);
  digitalWrite(10, HowMuch);
  digitalWrite(11, LOW);

}
/*---------------------------*/
void BackwardFast(int HowMuch)
{
  line_sense();
  digitalWrite(8, LOW); 
  digitalWrite(9, HowMuch);
  digitalWrite(10, HowMuch);
  digitalWrite(11, LOW);


}
/*---------------------------*/
void Stop()
{
  digitalWrite(8, HIGH); 
  digitalWrite(9, LOW);
  digitalWrite(10, LOW);
  digitalWrite(11, HIGH);
  
}
/*---------------------------*/
void SpinLeft(int HowMuch)
{
  line_sense();
  digitalWrite(8, HIGH); 
  digitalWrite(9, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(11, LOW);
  delay(HowMuch);
  Stop();


}
/*---------------------------*/
void SpinRight(int HowMuch)
{
   
  digitalWrite(8, LOW); 
  digitalWrite(9, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(11, HIGH);
  delay(HowMuch);
  Stop();

}
/*---------------------------*/
unsigned int PingBlink()
{
  uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  cm = uS / US_ROUNDTRIP_CM; // Convert ping time to distance in cm 
  Serial.print("  cm = ");   
  Serial.print(cm,DEC);  

  if ((cm < 40) && (cm != 0))
  {
    return(cm);
  }
  else
  {
    return(100);  // No Valid Distance
}// end PingBlink
}
/*---------------------------*/
void PointServo(int ServoAngle)
{

  for (int i=0;i<20;i++)  // Send the pulse 10 times 
  {
    digitalWrite(SERVO_PIN,HIGH);
    delayMicroseconds(ServoAngle);
    digitalWrite(SERVO_PIN,LOW);
    delay(20);
  }
}// PointServo end
/*---------------------------*/
void LookAround()  // Sets next state if Target Found
{
  for(int Direction = 0; Direction < DirectionsToLook ; Direction ++)
  {
    Serial.print("DIRECTION = ");     
    Serial.print(Direction,DEC);
    PointServo(ServoDirectionData[Direction]); // Get servo pulse width from array
    delay(100);
    cm_now = PingBlink();    // Read the Ultrasonic distance

    Serial.print(" cm_now = ");     
    Serial.println(cm_now,DEC);    
    if (cm_now < TARGET_TOO_CLOSE) digitalWrite(PIN13_LED,HIGH);    

    if (cm_now < 40)
    {
      TargetArray[TARGET_FOUND_ANY ] = true;
    }
    else TargetArray[TARGET_FOUND_ANY ] = false;

    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 0)) //LEFT
    {
        Stop();
        //BackwardFast(50);
        TargetArray[TARGET_LEFT ] = true;
        Serial.println("TargetLeft");

      }    
    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 1)) //Center
    {
        BackwardFast(100);
        TargetArray[TARGET_CENTER ] = true;
        Serial.println("TargetCenter");       
      }    
    if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 2)) //RIGHT
    {
        BackwardFast(100);
        TargetArray[TARGET_RIGHT ] = true;
        Serial.println("TargetRight");       
      }


  }// END Directions

}// END LookAround

/**************************( STATE MACHINE FUNCTIONS )******************************/
State RoboStartState()
{
  Serial.println("+++ RoboStartState");
  digitalWrite(PIN13_LED,LOW);    // LED Means Target Too Close
  ForwardSlow();  // Start moving forward
  delay(600);
  RoboGo.Set(RoboStopLookState);
}// END State RoboStartState


State RoboStopLookState()
{
  Serial.println("+++ RoboStopLookState");

  for (int i = 0; i < TARGET_ARRAY_SIZE; i++) TargetArray[i] = false;
  //Stop();  // Start moving forward
  LookAround();     // Ping Ultrasonic in different directions, Set TargetArray

  if      (TargetArray[TARGET_CENTER ] == true)  RoboGo.Set(eboTargetCenter);
  else if (TargetArray[TARGET_LEFT ]   == true)  RoboGo.Set(eboTargetLeft);
  else if (TargetArray[TARGET_RIGHT ]  == true)  RoboGo.Set(eboTargetRight);
  else     RoboGo.Set(RoboStartState);  

}// END State RoboStartState

State eboTargetLeft()
{
  Serial.println("***** eboTargetLeft");

  SpinLeft(600);
  //delay(1000);
  ForwardFast();
  delay(1000);
  line_sense();  

  RoboGo.Set(RoboStartState);  
}// END State RoboTargetLeft ----------------------

State eboTargetCenter()  

{
  Serial.println("***** eboTargetCenter");  
  //Stop();
  ForwardFast();
  ForwardFast();
  ForwardFast();
  delay(1000);
  line_sense();  

  // delay(500);

  RoboGo.Set(RoboStartState); 
} // END State RoboTargetCenter -----------------

State eboTargetRight()
{
  Serial.println("***** eboTargetRight");   
  SpinRight(600);

  ForwardFast();
  delay(1000);
  line_sense();

  RoboGo.Set(RoboStartState); 
}// END State RoboTargetRight -----------------
//LINE SENSOR FUNCTION
void line_sense(){
  int line1 = digitalRead(ls1);//left
    int line3 = digitalRead(ls3);//right
      if(line1 == 0)
      {
        if(a == 0){
          SpinLeft(980);
          Serial.println("ls1");
          a = 1;
        }else{
          SpinLeft(980);
          Serial.println("ls1");
          a = 0;
        }
      }
      if(line3 == 0)
      {
        if(a == 0){
          SpinLeft(980);
          Serial.println("ls3");
          b = 1;
        }else{
          SpinLeft(980);
          Serial.println("ls3");
          b = 0;
        }
      }

}

//*********( THE END )***********