Source Code

Robot source code

Version 24 - Released 26th of June 2022

#include <Arduino.h>

​

//#define PRINT_OUT

​

// Robot control parameters
bool bRunning = true;             // Is the robot in running wheel and cut motor mode?
bool m_bReadMicroSwitch = false;  // Read collision detection micro switch 
bool m_bCutMotorEnabled = true;
bool m_bwireSensorEnabled = true;
bool m_bUltraSonicSensorEnabled = true;

// Pin definitions

// Left/Right wheel motors
int iPinRightSpeedSensor = 3;
int pinRightMotorBack = 7; // define pin7 as left back connect with IN1
int pinRightMotorForward = 8; // define pin8 as left forward connect with IN2
int pinRightMotorSpeed=6;

int iPinLeftSpeedSensor = 2;
int pinLeftMotorBack = 9; // define pin9 as right back connect with IN3
int pinLeftMotorForward = 10; // define pin10 as right forward connect with IN4
int pinLeftMotorSpeed=11;

// Cut motor
int iPinCutMotorForward = 4;
int iPinCutMotorSpeed = 5;

// Ultra sonic
int m_iPinRightUltraSonicInput = A0; // define ultrasonic receive pin (Echo)
int m_iPinRightUltraSonicOutput = A1; // define ultrasonic send pin (Trig)
int m_iPinLeftUltraSonicInput = A2; // define ultrasonic receive pin (Echo)
int m_iPinLeftUltraSonicOutput = A3; // define ultrasonic send pin (Trig)

// Collision micro switch
int m_iPinMicroSwitch = 13;

// Start/Stop button.
int m_iPinStartStopButton = 12; // Orange = Gnd, Green = V, Yellow = S

// Wheel motor speed parameters
int m_iLeftMotorSpeed = 0;  // Set Left engine start speed (0-255)
int m_iRightMotorSpeed = 0; // Set Right engine start speed (0-255)
int m_iTopMotorSpeed = 255;
int m_iLowestMotorSpeed = 200;
int m_iTopLeftSpeed = 255;
int m_iTopRightSpeed = 255;
int m_iSpeedChange = 25;
int m_iSpeedChangeDelay = 10;
int m_iFastSpeedChangeDelay = 1;

// Wheel motor speed sensor parameters
static const float m_scfWheelCircumferenceInCm = 44.0f;
static const float m_scfGearRatio = 150.0f;
static const float m_scfPulsesPerRevolution = 11.0f * m_scfGearRatio;
static const float m_scfDistancePerPulsInCm = m_scfWheelCircumferenceInCm / m_scfPulsesPerRevolution;
bool m_bNewLeftSpeedSensorPulse = false;
bool m_bNewRightSpeedSensorPulse = false;
unsigned long m_ulWaitTimeSpeedSensor = 0;

unsigned long m_ulLeftLastSensorInterruptTime = 0;
float m_fLeftTimePerPulseInSec = 0.0f;
float m_fLeftSensorSpeedInCmPerSec = 0.0f;         // Left motor encoder estimated speed

unsigned long m_ulRightLastSensorInterruptTime = 0;
float m_fRightTimePerPulseInSec = 0.0f;
float m_fRightSensorSpeedInCmPerSec = 0.0f;         // Right motor encoder estimated speed;

static const unsigned long m_sculTimeBeforStartingSpeedSensorInMs = 1500; // 800 Wait time before it start checking the motor speed
float m_fMotorSpeedForBeingStuckInCmPerSec = 10.0f; // 15.0f Parameter used to identify if a robot motor is stuck
bool m_bCheckMotorSpeedForBeingStuck = false;
int m_iMotorSpeedChange = 1;

// Cut motor parameters
int m_iCutMotorSpeed = 255; // Engine speed (0-255)

// Ultra sound sensor parameters
unsigned long m_ulPulseSendTime = 0; // Time for last pulse send time
unsigned long m_ulTimeBetweenPulses = 10000;
unsigned long m_ulMinimumPulseSendWaitTime = 500; // Minimum wait time before start listen for echo from sent pulse
bool m_bLeftPulseSent = false;
bool m_bLeftPulseStarted = false;
int m_iLeftNumOfPulsesInRow = 0;
unsigned long m_ulLeftPulseStartTime;
bool m_bRightPulseSent = false;
bool m_bRightPulseStarted = false;
int m_iRightNumOfPulsesInRow = 0;
unsigned long m_ulRightPulseStartTime;
unsigned long m_ulMaxPulseTimeInMs = 2000;
unsigned long m_ulAvoidDistance = 1200; // 1000 = 10cm

// Digital fence parameters
bool m_bLeftSensorRead = true;
int iLowPulseValue = 400; //400
int iHighPulseValue = 750; //750
int iPulseValueIndex = 0;
int iMediumPulseValue = 0;
static const unsigned long m_sculMinFencePulseTimeMs = 70; //70, 95
static const unsigned long m_sculMaxFencePulseTimeMs = 90; //90, 110
static const unsigned long m_sculTimeBeforStartingReadingFensePulseInMs = 1500; // 800
unsigned long m_ulWaitTimeFencePulse = 0;

struct FenceSensor
{
  int iSensorReadValue;
  bool bPulseStarted = false;
  unsigned long ulFencePulseStartTime;
  bool bPulseDetected = false;
};

FenceSensor m_leftFenceSensor;
FenceSensor m_rightFenceSensor;

// Robot action constants and status
const int ciStopped = 0;
const int ciForward = 1;
const int ciBack = 2;
const int ciRight = 3;
const int ciLeft = 4;
const int ciFastBack = 5;
const int ciFastStop = 6;
const int ciBackLeft = 7;
const int ciBackRight = 8;
int m_iDirection = ciForward;
int m_iSetMotorDirection = ciStopped;

​

// Setup robot microcontroller initial state.
void setup()
{  
  Serial.begin(57600);
  Serial.println("Begin setup");

  // Left wheel motor 
  pinMode(iPinLeftSpeedSensor, INPUT);
  attachInterrupt(digitalPinToInterrupt(iPinLeftSpeedSensor), LeftSpeedSensorInterrupt, RISING);
  pinMode(pinLeftMotorBack, OUTPUT);
  pinMode(pinLeftMotorForward, OUTPUT);
  pinMode(pinLeftMotorSpeed, OUTPUT);
  analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed);

  // Right wheel motor
  pinMode(iPinRightSpeedSensor, INPUT);
  attachInterrupt(digitalPinToInterrupt(iPinRightSpeedSensor), RightSpeedSensorInterrupt, RISING);
  pinMode(pinRightMotorBack, OUTPUT);
  pinMode(pinRightMotorForward, OUTPUT);
  pinMode(pinRightMotorSpeed, OUTPUT);
  analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed);

  // Cut motor pin setup
  pinMode(iPinCutMotorForward, OUTPUT);
  pinMode(iPinCutMotorSpeed, OUTPUT);
  analogWrite(iPinCutMotorSpeed, m_iCutMotorSpeed);

  // Ultra sound sensors pin setup
  pinMode(m_iPinLeftUltraSonicInput, INPUT);
  pinMode(m_iPinLeftUltraSonicOutput, OUTPUT);
  digitalWrite(m_iPinLeftUltraSonicOutput, HIGH);

  pinMode(m_iPinRightUltraSonicInput, INPUT);
  pinMode(m_iPinRightUltraSonicOutput, OUTPUT);
  digitalWrite(m_iPinRightUltraSonicOutput, HIGH);

  // Setup fast analog pin A4 and A5 read.

  // ADMUX – ADC Multiplexer Selection Register
  // Bits:  REFS1  REFS0  ADLAR  ----  MUX3  MUX2  MUX1  MUX0
  // REFS1 = 0, REFS0 = 1 ->  AVCC with external capacitor at AREF pin
  // MUX3 = 0, MUX2 = 1, MUX1 = 0, MUX0 = 0 -> Using ADC4 = Pin A4
  ADMUX = 0B01000100; // use pin A4 for analog read
  // ADMUX = 0B01000101; // use pin A5 for analog read

  // DIDR0 – Digital Input Disable Register 0
  // Bits:  –--- –---- ADC5D ADC4D ADC3D ADC2D ADC1D ADC0D
  // This register is used to disable digital input on ADC0 to ADC5 (A0 to A5, bit 0 = A0, bit 1 = A1, bit 2 = A2, bit 3 = A3, bit 4 = A4, bit 5 = A5)
  // When the bit is written logic one, the digital input buffer on the corresponding ADC pin is disabled. 
  DIDR0 = 0B00110000; // Turn off digital input for ADC5 and ADC4 since we will only use them for analog read which will speed them up.
 
  // ADCSRA - ADC Control and Status Register A
  // Bits: ADEN  ADSC  ADATE  ADIF  ADIE  ADPS2  ADPS1  ADPS0
  // ADEN: ADC Enable
  // ADSC: Start conversion. In Free Running mode, write this bit to one to start the first conversion.
  // ADATE: ADC Auto Trigger Enable
  // ADIF: ADC Interrupt Flag
  // ADIE: ADC Interrupt Enable
  // ADPS[2:0]: ADC Prescaler Select Bits
  //ADCSRA = 0B11100111;
  ADCSRA = 0B11011101; // ADC enable, ADSC start conversion, manual trigger mode, ADC interrupt enable, prescaler = 128

  // Bits: ADEN  ADSC  ADATE  ADIF  ADIE  ADPS2  ADPS1  ADPS0
  ADCSRB = 0x00; // Set the ADC to free running mode
 
  // Collision micro switch pin setup
  pinMode(m_iPinMicroSwitch, INPUT);

  // Start/Stop button pin setup
  pinMode(m_iPinStartStopButton, INPUT);

#ifdef PRINT_OUT
  Serial.println("Return setup");
#endif

  RunCutMotor();
}

​

void SetCurrentSpeed(int iSpeed)
{
      m_iLeftMotorSpeed = iSpeed;
      m_iRightMotorSpeed = iSpeed;
}

​

void softSpeedUp()
{
  if (m_iLeftMotorSpeed < m_iTopLeftSpeed || m_iRightMotorSpeed < m_iTopRightSpeed)
  {
    while ( m_iLeftMotorSpeed < m_iTopLeftSpeed || m_iRightMotorSpeed < m_iTopRightSpeed )
    {
      if ( m_iLeftMotorSpeed < m_iTopLeftSpeed )
      {
        m_iLeftMotorSpeed += m_iSpeedChange;
        if ( m_iLeftMotorSpeed > m_iTopLeftSpeed)
          m_iLeftMotorSpeed = m_iTopLeftSpeed;
        delay(m_iSpeedChangeDelay);
      }
      if ( m_iRightMotorSpeed < m_iTopRightSpeed )
      {
        m_iRightMotorSpeed += m_iSpeedChange;
        if ( m_iRightMotorSpeed > m_iTopRightSpeed)
          m_iRightMotorSpeed = m_iTopRightSpeed;
        delay(m_iSpeedChangeDelay);
      }
      analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed); // Set Left motor speed (0-255)
      analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed); // Set Right motor speed (0-255)
  #ifdef PRINT_OUT
      Serial.println("SoftSpeedUp 1");
      Serial.print(m_iLeftMotorSpeed);
      Serial.println(" = Left speed");
      Serial.print(m_iRightMotorSpeed);
      Serial.println(" = Right speed");
  #endif
    }
    AddSensorWaitTimeWhenSpeedingUp();
  }
  else
  {     
    // Make sure that the Left motor is not going faster than set Top speed
    if (m_iLeftMotorSpeed > m_iTopLeftSpeed)
    {
      m_iLeftMotorSpeed = m_iTopLeftSpeed;
      analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed); // Set Left engine speed (0-255)
  #ifdef PRINT_OUT
      Serial.println("SoftSpeedUp 2");
      Serial.print(m_iLeftMotorSpeed);
      Serial.println(" = Left speed");
      Serial.print(m_iRightMotorSpeed);
      Serial.println(" = Right speed");
  #endif
    }
    
    // Make sure that the Right motor is not going faster than set Top speed
    if (m_iRightMotorSpeed > m_iTopRightSpeed)
    {
      m_iRightMotorSpeed = m_iTopRightSpeed;
      analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed); // Set Reft engine speed (0-255)
  #ifdef PRINT_OUT
      Serial.println("SoftSpeedUp 3");
      Serial.print(m_iLeftMotorSpeed);
      Serial.println(" = Left speed");
      Serial.print(m_iRightMotorSpeed);
      Serial.println(" = Right speed");
  #endif
    }
  }
}

​

void softSpeedDown(int iSpeedChangeDelay)
{
  while ( m_iLeftMotorSpeed > 0 || m_iRightMotorSpeed > 0 )
  {
    if ( m_iLeftMotorSpeed > 0 )
    {
      m_iLeftMotorSpeed -= m_iSpeedChange;
      if ( m_iLeftMotorSpeed < 0)
        m_iLeftMotorSpeed = 0;
      delay(iSpeedChangeDelay);
    }
    if ( m_iRightMotorSpeed > 0 )
    {
      m_iRightMotorSpeed -= m_iSpeedChange;
      if ( m_iRightMotorSpeed < 0)
        m_iRightMotorSpeed = 0;
      delay(iSpeedChangeDelay);
    }
    analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed); // Set Left motor speed (0-255)
    analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed); // Set Right motor speed (0-255)
  }
}

​

void forward(int iTime) // Run the robot forward for a given time in milli seconds
{
  if (m_iSetMotorDirection != ciForward)
  {
    digitalWrite(pinRightMotorBack,LOW);
    digitalWrite(pinRightMotorForward,HIGH);
    digitalWrite(pinLeftMotorBack,LOW);
    digitalWrite(pinLeftMotorForward,HIGH);
    m_iSetMotorDirection = ciForward;
  }
 
  softSpeedUp();
  if (iTime != 0)
    delay(iTime);
}

​

void turnRight(int iTime) // Turn the robot to the right for a given time in milli seconds
{
  digitalWrite(pinRightMotorBack,LOW);
  digitalWrite(pinRightMotorForward,HIGH);
  digitalWrite(pinLeftMotorBack,HIGH);
  digitalWrite(pinLeftMotorForward,LOW);
  m_iSetMotorDirection = ciRight;
  softSpeedUp();
  delay(iTime);
  softSpeedDown(m_iSpeedChangeDelay);
}

​

void turnLeft(int iTime) // Turn the robot to the left for a given time in milli seconds
{
  digitalWrite(pinRightMotorBack,HIGH);
  digitalWrite(pinRightMotorForward,LOW);
  digitalWrite(pinLeftMotorBack,LOW);
  digitalWrite(pinLeftMotorForward,HIGH);
  m_iSetMotorDirection = ciLeft;
  softSpeedUp();
  delay(iTime);
  softSpeedDown(m_iSpeedChangeDelay);
}

​

void stopp(int iTime) // Stop the robot and wait for a given time in milli seconds
{
  softSpeedDown(m_iSpeedChangeDelay);
  digitalWrite(pinRightMotorBack,HIGH);
  digitalWrite(pinRightMotorForward,HIGH);
  digitalWrite(pinLeftMotorBack,HIGH);
  digitalWrite(pinLeftMotorForward,HIGH);
  m_iSetMotorDirection = ciStopped;
  delay(iTime);
}

​

void fastStop(int iTime) // Fast stop when collision detected and wait for a given time in milli seconds
{
  digitalWrite(pinRightMotorBack,HIGH);
  digitalWrite(pinRightMotorForward,HIGH);
  digitalWrite(pinLeftMotorBack,HIGH);
  digitalWrite(pinLeftMotorForward,HIGH);
  m_iSetMotorDirection = ciFastStop;
  delay(iTime);
}

​

void back(int iTime) // Run the robot backwards for a given time in milli seconds
{
  softSpeedDown(m_iSpeedChangeDelay);
  digitalWrite(pinRightMotorBack,HIGH);
  digitalWrite(pinRightMotorForward,LOW);
  digitalWrite(pinLeftMotorBack,HIGH);
  digitalWrite(pinLeftMotorForward,LOW);
  softSpeedUp();
  delay(iTime);
}

​

void RunCutMotor()
{
  if (m_bCutMotorEnabled)
    digitalWrite(iPinCutMotorForward, HIGH); // Run the cut motor.
  else
    digitalWrite(iPinCutMotorForward, LOW); // Stop the cut motor.
}

​

void StopCutMotor()
{
  digitalWrite(iPinCutMotorForward, LOW); // Stop the cut motor.
}

void BackOffManeuver(int iDirection = ciBack)
{
  if (m_iDirection == ciForward)
  {
    m_iDirection = iDirection;
  }
}

​

void FastBackOffManeuver()
{
  if (m_iDirection == ciForward)
  {
    m_iDirection = ciFastBack;
  }
}

​

void AlignLeftRightMotorSpeed()
{
#ifdef PRINT_OUT
  Serial.print(m_fLeftSensorSpeedInCmPerSec, 8);
  Serial.println(" = Left sensor speed in cm per sec");
  Serial.print(m_fRightSensorSpeedInCmPerSec, 8);
  Serial.println(" = Right sensor speed in cm per sec");
#endif

  if (m_fLeftSensorSpeedInCmPerSec < m_fRightSensorSpeedInCmPerSec)
  {
    if (m_iTopLeftSpeed < m_iTopMotorSpeed)
    {
      m_iTopLeftSpeed += m_iMotorSpeedChange;
      m_iLeftMotorSpeed = m_iTopLeftSpeed;
      analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed); // Set Left motor speed (0-255)
#ifdef PRINT_OUT
      Serial.println("Set higher left speed");
#endif
    }
    else
    {
      m_iTopRightSpeed -= m_iMotorSpeedChange;
      if (m_iTopRightSpeed < m_iLowestMotorSpeed)
        m_iTopRightSpeed = m_iLowestMotorSpeed;
      m_iRightMotorSpeed = m_iTopRightSpeed;
      analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed); // Set Right motor speed (0-255)
#ifdef PRINT_OUT
      Serial.println("Set lower right speed");
#endif
    }
  }
  else
  {
    if (m_fRightSensorSpeedInCmPerSec < m_fLeftSensorSpeedInCmPerSec)
    {
      if (m_iTopRightSpeed < m_iTopMotorSpeed)
      {
        m_iTopRightSpeed += m_iMotorSpeedChange;
        m_iRightMotorSpeed = m_iTopRightSpeed;
        analogWrite(pinRightMotorSpeed, m_iRightMotorSpeed); // Set Right motor speed (0-255)
#ifdef PRINT_OUT
        Serial.println("Set higher right speed");
#endif
      }
      else
      {
        m_iTopLeftSpeed -= m_iMotorSpeedChange;
        if (m_iTopLeftSpeed < m_iLowestMotorSpeed)
          m_iTopLeftSpeed = m_iLowestMotorSpeed;
        m_iLeftMotorSpeed = m_iTopLeftSpeed;
        analogWrite(pinLeftMotorSpeed, m_iLeftMotorSpeed); // Set Left mogtor speed (0-255)
#ifdef PRINT_OUT
        Serial.println("Set lower left speed");
#endif
      }
    }
  }

//#ifdef PRINT_OUT
  static int iCounter = 0;
  iCounter++;
  if (iCounter == 10)
  {
    Serial.print(m_fLeftSensorSpeedInCmPerSec, 8);
    Serial.println(" = Left sensor speed in cm per sec");
    Serial.print(m_iLeftMotorSpeed);
    Serial.println(" = Left speed");
    Serial.print(m_fRightSensorSpeedInCmPerSec, 8);
    Serial.println(" = Right sensor speed in cm per sec");
    Serial.print(m_iRightMotorSpeed);
    Serial.println(" = Right speed");
    iCounter = 0;
  }
//#endif
}

​

void LeftSpeedSensorInterrupt()
{
  unsigned long ulCurrentTime;

#ifdef PRINT_OUT
  Serial.print("LeftSpeedSensorInterrupt"); 
#endif
 
  ulCurrentTime = micros();

  // Make sure it´s not a bounce, should be more than 5 millisec between pulses.
//  if (ulCurrentTime - m_ulLeftLastSensorInterruptTime > 10000)
  {
    m_fLeftTimePerPulseInSec = (float)(ulCurrentTime - m_ulLeftLastSensorInterruptTime) / 1000000.0f;
    m_ulLeftLastSensorInterruptTime = ulCurrentTime;
    m_bNewLeftSpeedSensorPulse = true;
  }
}

​

void RightSpeedSensorInterrupt()
{
  unsigned long ulCurrentTime;

#ifdef PRINT_OUT
  Serial.print("RightSpeedSensorInterrupt");
#endif
 
  ulCurrentTime = micros();

  // Make sure it´s not a bounce, should be more than 5 millisec between pulses.
//  if (ulCurrentTime - m_ulRightLastSensorInterruptTime > 10000)
  {
    m_fRightTimePerPulseInSec = (float)(ulCurrentTime - m_ulRightLastSensorInterruptTime) / 1000000.0f;
    m_ulRightLastSensorInterruptTime = ulCurrentTime;
    m_bNewRightSpeedSensorPulse = true;
  } 
}

​

void AddSensorWaitTimeWhenSpeedingUp()
{
  m_ulWaitTimeSpeedSensor = millis() + m_sculTimeBeforStartingSpeedSensorInMs;
  m_ulLeftLastSensorInterruptTime = m_ulWaitTimeSpeedSensor;
  m_ulRightLastSensorInterruptTime = m_ulWaitTimeSpeedSensor;

  m_leftFenceSensor.bPulseStarted = false;
  m_rightFenceSensor.bPulseStarted = false;
 
  m_ulWaitTimeFencePulse = millis() + m_sculTimeBeforStartingReadingFensePulseInMs;
}

​

void ReadMotorSensorSpeed()
{
  if (millis() > m_ulWaitTimeSpeedSensor &&  m_bNewLeftSpeedSensorPulse && m_bNewRightSpeedSensorPulse)
  {
    m_bNewLeftSpeedSensorPulse = false;
    m_bNewRightSpeedSensorPulse = false;
 
    m_fLeftSensorSpeedInCmPerSec = m_scfDistancePerPulsInCm / m_fLeftTimePerPulseInSec;
    m_fLeftSensorSpeedInCmPerSec *= 0.96f; // Compensate for left motor encoder not showing correct speed
    m_fRightSensorSpeedInCmPerSec = m_scfDistancePerPulsInCm / m_fRightTimePerPulseInSec;
 
    AlignLeftRightMotorSpeed();
   
    // Identify if the robot is stuck by checking if the sensor speed is lower than the m_fMotorSpeedForBeingStuckInCmPerSec.
    if (m_bCheckMotorSpeedForBeingStuck && m_fLeftSensorSpeedInCmPerSec < m_fMotorSpeedForBeingStuckInCmPerSec)
    {
//  #ifdef PRINT_OUT
      Serial.print("Left motor speed sensor detected low motor speed, Speed = ");
      Serial.println(m_fLeftSensorSpeedInCmPerSec);
//  #endif
      FastBackOffManeuver();
    }
 
    // Set right motor speed
    
    // Identify if the robot is stuck by checking if the sensor speed is lower than the m_fMotorSpeedForBeingStuckInCmPerSec.
    if (m_bCheckMotorSpeedForBeingStuck && m_fRightSensorSpeedInCmPerSec < m_fMotorSpeedForBeingStuckInCmPerSec)
    {
//  #ifdef PRINT_OUT
      Serial.print("Right motor speed sensor detected low motor speed, Speed = ");
      Serial.println(m_fRightSensorSpeedInCmPerSec);
//  #endif
      FastBackOffManeuver();
    }
  }
}

​

void ReadUltraSonicDistance()
{
  unsigned long ulCurrentTime;
  unsigned long ulTimeElapsed;

  ulCurrentTime = micros();
 
  if (ulCurrentTime > (m_ulPulseSendTime + m_ulTimeBetweenPulses))
  {
    // Send left side ultra sonic pulse
    digitalWrite(m_iPinLeftUltraSonicOutput, LOW);
    digitalWrite(m_iPinRightUltraSonicOutput, LOW);
    delayMicroseconds(1);
    digitalWrite(m_iPinLeftUltraSonicOutput, HIGH);
    digitalWrite(m_iPinRightUltraSonicOutput, HIGH);
    m_ulPulseSendTime = micros();
    m_bLeftPulseSent = true;
    m_bRightPulseSent = true;
  }
 
  // Check for left ultra sonic pulse
   ulCurrentTime = micros();
  if (m_bLeftPulseStarted)
  {
    ulTimeElapsed = ulCurrentTime - m_ulLeftPulseStartTime;
    if (ulTimeElapsed > m_ulAvoidDistance)
    {
#ifdef PRINT_OUT
       Serial.println("Left max pulse time");
#endif
       m_bLeftPulseStarted = false;
       m_bLeftPulseSent = false;
       m_iLeftNumOfPulsesInRow = 0;
    }
    else
    {
      if (digitalRead(m_iPinLeftUltraSonicInput) == LOW)
      {
        if (ulTimeElapsed < m_ulAvoidDistance)
        {
          m_iLeftNumOfPulsesInRow++;
          if (m_iLeftNumOfPulsesInRow > 2)
          {
#ifdef PRINT_OUT
            Serial.println("Left ultra sonic collision detected");
#endif
            m_iLeftNumOfPulsesInRow = 0;
            BackOffManeuver(ciBackLeft);
          }
        }
        m_bLeftPulseStarted = false;
        m_bLeftPulseSent = false;
      }
    }
  }
  else
  {
    if (m_bLeftPulseSent && ulCurrentTime > (m_ulPulseSendTime + m_ulMinimumPulseSendWaitTime) && digitalRead(m_iPinLeftUltraSonicInput) == HIGH)
    {
#ifdef PRINT_OUT
      Serial.println("Left pulse started");
#endif
      m_bLeftPulseStarted = true;
      m_ulLeftPulseStartTime = ulCurrentTime;
    }
  }

  // Check for right ultra sonic pulse
  ulCurrentTime = micros();
  if (m_bRightPulseStarted)
  {
    ulTimeElapsed = ulCurrentTime - m_ulRightPulseStartTime;
    if (ulTimeElapsed > m_ulAvoidDistance)
    {
#ifdef PRINT_OUT
       Serial.println("Right max pulse time");
#endif
       m_bRightPulseStarted = false;
       m_bRightPulseSent = false;
       m_iRightNumOfPulsesInRow = 0;
    }
    else
    {
      if (digitalRead(m_iPinRightUltraSonicInput) == LOW)
      {
        if (ulTimeElapsed < m_ulAvoidDistance)
        {
          m_iRightNumOfPulsesInRow++;
          if (m_iRightNumOfPulsesInRow > 2)
          {
#ifdef PRINT_OUT
            Serial.println("Right ultra sonic collision detected");
#endif
            m_iRightNumOfPulsesInRow = 0;
            BackOffManeuver(ciBackRight);
          }
        }
        m_bRightPulseStarted = false;
        m_bRightPulseSent = false;
      }
    }
  }
  else
  {
    if (m_bRightPulseSent && ulCurrentTime > (m_ulPulseSendTime + m_ulMinimumPulseSendWaitTime) && digitalRead(m_iPinRightUltraSonicInput) == HIGH)
    {
#ifdef PRINT_OUT
      Serial.println("Right pulse started");
#endif
      m_bRightPulseStarted = true;
      m_ulRightPulseStartTime = ulCurrentTime;
    }
  }
}

​

void DetectVirtualFencePulse(FenceSensor &fenceSensor)
{
  unsigned long ulTime;

#ifdef PRINT_OUT
  Serial.print(iFenceReadValue);
  Serial.println(" = value, Read");
#endif

  if (millis() > m_ulWaitTimeFencePulse && !fenceSensor.bPulseStarted)
  {
    if (fenceSensor.iSensorReadValue > iHighPulseValue)
    {
#ifdef PRINT_OUT
      Serial.print(fenceSensor.iSensorReadValue);
      Serial.println(" = value, Fence pulse started");
#endif
      fenceSensor.bPulseStarted = true;
      fenceSensor.ulFencePulseStartTime = micros();
    }
  }
  else
  {
    // Has it been to long since the puls started?
    ulTime = micros();
    if (fenceSensor.bPulseStarted && ulTime > fenceSensor.ulFencePulseStartTime + m_sculMaxFencePulseTimeMs)
    {
#ifdef PRINT_OUT
      Serial.print(ulTime - (fenceSensor.ulFencePulseStartTime + m_sculMaxFencePulseTimeMs));
      Serial.println(" = To long since the puls started");
#endif
      fenceSensor.bPulseStarted = false;
    }
    if (fenceSensor.bPulseStarted)
    {
      if (fenceSensor.iSensorReadValue < iLowPulseValue && ulTime > fenceSensor.ulFencePulseStartTime + m_sculMinFencePulseTimeMs)
      {
        unsigned int uiTime = (unsigned int)(ulTime - fenceSensor.ulFencePulseStartTime);
#ifdef PRINT_OUT
        Serial.print(fenceSensor.iSensorReadValue);
        Serial.println(" = value, Fence pulse ended");
        Serial.print(uiTime);
        Serial.println(" = time between pulse start and end");
#endif
        fenceSensor.bPulseDetected = true;
        fenceSensor.bPulseStarted = false;
      }
    }
  }
}

​

// Interrupt service routine for ADC conversion complete
ISR(ADC_vect) 
{
  // Read analog value
  uint8_t ui8Low, ui8High;
  ui8Low = ADCL;
  ui8High = ADCH;

#ifdef PRINT_OUT
    Serial.println("Interrupt service routine for ADC conversion called");
#endif

  if (m_bwireSensorEnabled)
  {
    if (m_bLeftSensorRead)
    {
      m_leftFenceSensor.iSensorReadValue = (ui8High << 8) | ui8Low;
#ifdef PRINT_OUT
        Serial.print("Left fence sensor value = ");
        Serial.println(m_leftFenceSensor.iSensorReadValue);
#endif
      DetectVirtualFencePulse(m_leftFenceSensor);
      ADMUX = 0B01000101; // Switch to use pin A5 for next analog read
    }
    else
    {
      m_rightFenceSensor.iSensorReadValue = (ui8High << 8) | ui8Low;
#ifdef PRINT_OUT
        Serial.print("Right fence sensor value = ");
        Serial.println(m_rightFenceSensor.iSensorReadValue);
#endif
      DetectVirtualFencePulse(m_rightFenceSensor);
      ADMUX = 0B01000100; // Switch to use pin A4 for next analog read
    }
    m_bLeftSensorRead = !m_bLeftSensorRead;
  }
    
  ADCSRA |= 0B01000000; // Start new ADC conversion.
}

​

void HandleBackOffManeuver()
{
  long lRandomNum;
  long lRandomTurnTime;
 
  // Is the robot backing out from an obstacle?
  if(m_iDirection == ciBack || m_iDirection == ciFastBack || m_iDirection == ciBackLeft || m_iDirection == ciBackRight)
  {
#ifdef PRINT_OUT
    Serial.println("Go back and then turn");
#endif
    if (m_iDirection == ciFastBack)
      fastStop(50);
    else
      stopp(100);
    back(300);
    stopp(100);
    lRandomTurnTime = random(400, 600);
    if (m_iDirection == ciBack || m_iDirection == ciFastBack)
    {
      lRandomNum = random(2);
      if (lRandomNum == 0)
        turnRight(lRandomTurnTime);
      else
        turnLeft(lRandomTurnTime);
    }
    else
    {
      if (m_iDirection == ciBackLeft)
      {
#ifdef PRINT_OUT
        Serial.println("Go back and then turn left");
#endif
        turnLeft(lRandomTurnTime);
      }
      if (m_iDirection == ciBackRight)
      {
#ifdef PRINT_OUT
        Serial.println("Go back and then turn right");
#endif
        turnRight(lRandomTurnTime);
      }
    }
    SetCurrentSpeed(0);
    m_iDirection = ciForward;
#ifdef PRINT_OUT
    Serial.println("Go forward");
#endif
    
    // Reset any fence pulse detection to stop double backing maneuver.
    m_leftFenceSensor.bPulseDetected = false;
    m_leftFenceSensor.bPulseStarted = false;
    m_rightFenceSensor.bPulseDetected = false;
    m_rightFenceSensor.bPulseStarted = false;
  }
}

​

void loop()
{
  // Check for collisions or objects in the way.
  if (m_bUltraSonicSensorEnabled)
    ReadUltraSonicDistance();
    
  // Check if close to virtual fence.
  if (m_rightFenceSensor.bPulseDetected)
  {
    BackOffManeuver(ciBackLeft);
  }
  if (m_leftFenceSensor.bPulseDetected)
  {
    BackOffManeuver(ciBackRight);
  }

  if (bRunning)
  {
    if(m_bReadMicroSwitch && digitalRead(m_iPinMicroSwitch) == LOW)
    {
#ifdef PRINT_OUT
      Serial.println("Micro switch collision detected");
#endif
      FastBackOffManeuver();
    }

    // Is the robot going forward?
    if(m_iDirection == ciForward)
    {
      forward(0);
      ReadMotorSensorSpeed();
    }

    HandleBackOffManeuver();
  }
  if(digitalRead(m_iPinStartStopButton) == HIGH)
  {
    bRunning = !bRunning;
    if ( bRunning )
    {
      delay(500); // Wait 500 milliseconds before the robot starts.
#ifdef PRINT_OUT
      Serial.println("Start running motors");
#endif
      RunCutMotor();
    }
    else
    {
#ifdef PRINT_OUT
      Serial.println("Stop running motors");
#endif
      StopCutMotor();
      stopp(100);
      SetCurrentSpeed(0);
      m_iDirection = ciForward;
    }
  }
}