e-Bot_4x4_Sumobot.ino

/*
   e-Gizmo e-Bot 4x4 Sumobot
   (with US-100 sensor)

   Using State Machine library of Arduino.
   For e-BOT 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  (not used in this demo)
  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"
#include "State.h"
// State Machine library

// Ultrasonic Sensor Pins
#define TRIGGER_PIN  12  // trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3   // echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance (in centimeters). Maximum distance is rated at 400-500cm.

// Servo pin
#define SERVO_PIN    7  // to look around or neck of US -100 sensor
#define PIN13_LED    13  // on-board LED indicator

// Target Array (if target found distance)
#define TARGET_FOUND_ANY     0   // Values will be "1" or "0"
#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 eBotGo(eBotStart);//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 = 4;  // For LookAround()
int ServoDirectionData[4] = { 1600, 2000, 900, 1600};

int runspeed=255;



void setup()   /******************* SETUP: RUNS ONCE *****************/
{
  Serial.begin(115200);
  delay(1000);
  Serial.println("Gizduino e-Bot (Sumo) Test");
  
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(SERVO_PIN,OUTPUT);  
  pinMode(PIN13_LED,OUTPUT);

}//--(end setup )---

/************** LOOP: RUNS CONSTANTLY **************************/
void loop()   
{
  EXEC(eBotGo);//run statemachine
  delay(100);
}//--(end main loop )---


/*----------------( Declare User-written Functions )---------------*/

//------( MOTOR CONTROL FUNCTIONS )----------------
void ForwardSlow()
{
 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()
{
 digitalWrite(8, HIGH); 
 digitalWrite(9, HIGH);
 digitalWrite(10, HIGH);
 digitalWrite(11, HIGH);

}
/*---------------------------*/

void BackwardSlow(int runspeed)
{
 digitalWrite(8, LOW); 
 digitalWrite(9, runspeed);//run2
 digitalWrite(10, runspeed);//run1
 digitalWrite(11, LOW);
 delay(1000);

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

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

}
/*---------------------------*/
void SpinRight(int runspeed)
{
 digitalWrite(8, LOW); 
 digitalWrite(9, HIGH);
 digitalWrite(10, HIGH);
 digitalWrite(11, HIGH);
 delay(runspeed);
 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();
      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 eBotStart()
{
  Serial.println("+++ eBotStart");
  digitalWrite(PIN13_LED,LOW);    // LED Means Target Too Close
  ForwardSlow();  // Start moving forward
  delay(600);
  
  eBotGo.Set(eBotStoptoLook);    

}// END State eBotStart


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

  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)  eBotGo.Set(eboTargetCenter);
  else if (TargetArray[TARGET_LEFT ]   == true)  eBotGo.Set(eboTargetLeft);
  else if (TargetArray[TARGET_RIGHT ]  == true)  eBotGo.Set(eboTargetRight);
  else     eBotGo.Set(eBotStart);  

}// END State eBotStart

State eboTargetLeft()
{
  Serial.println("***** eboTargetLeft");
  
  SpinLeft(600);
  ForwardFast();
  delay(1000);

  eBotGo.Set(eBotStart);  
}// END State RoboTargetLeft ----------------------

State eboTargetCenter()  
{
  Serial.println("***** eboTargetCenter");  
  ForwardFast();
  ForwardFast();
  ForwardFast();
  delay(1000);

  eBotGo.Set(eBotStart); 
} // END State RoboTargetCenter -----------------

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

  eBotGo.Set(eBotStart); 
}// END State RoboTargetRight -----------------

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