Elisa3 Robot Bouncing off walls

Here I have programmed the Elisa3 robot to detect the black edges of the paper and retreat and spun away. This uses the infrared ground sensors below the robot. The motors are bit sticky so turning is somewhat random, but the edge sensor algorithm works correctly.

The code is a modification of the code I used to follow a black line, but this time when one of the Infrared ground sensors senses black, the robot does a spin away, changing direction.

Code

//Line Tracking with Elisa-3 Robot

//PJ1 & PJ2 are ground sensors
//ADC 9 and 10 are the IR receivers
//RGB LEDs are PB5, PB6 and PB7

//****************************************
//Declare global variables****************
//****************************************
int RawSensorReading = 0;
const int Threshold = 900; //Set the threshold for black
unsigned char SensorStates[3] = {0}; //we use array values 1 and 2 to match the Ground Sensors


enum DirectionRobotNeedsToGo{Forward, Stop, Left, Right, Back}; //Maps to 0, 1, 2, 3
enum DirectionRobotNeedsToGo CurrentRobotDirection;

enum SettheRGBColour{RED, GREEN, BLUE, CYAN, MAGENTA, YELLOW};
enum SettheRGBColour CurrentRGBColour; 

float Speed = 12; //This represents the duty cycle


//****************************************
//****************************************
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  DDRJ |= (1<<PJ1)|(1<<PJ2);  //data directions register for ground IR-LEDs
  DDRB |= (1<<PB5)|(1<<PB6)|(1<<PB7); //data direction register for green LEDs
  CurrentRGBColour = GREEN;
}

//****************************************
//MAIN LOOP BELOW*************************
//****************************************
void loop() {
  // put your main code here, to run repeatedly:
  ReadGroundLED(2);
  ReadGroundLED(1);
  CalculateRobotState(SensorStates[1],SensorStates[2]);
  SetRGBLEDs(); //colour is set by CurrentRGBColour
  ActOnRobotState();
 delay(50);
}
//****************************************
//MAIN LOOP ENDS**************************
//****************************************

//****************************************
//SET RGB COLOUR**************************
//****************************************
void SetRGBLEDs()
{
  switch(CurrentRGBColour)
  {
    case RED:
      PORTB &= ~(1<<PB5); //R on
      PORTB |= ((1<<PB6)|(1<<PB7)); //G & B off
    break;

    case GREEN:
      PORTB &= ~(1<<PB6); //G on
      PORTB |= ((1<<PB5)|(1<<PB7)); //R & B off
    break;

    case BLUE:
      PORTB &= ~(1<<PB7); //B on
      PORTB |= ((1<<PB5)|(1<<PB6)); //R & G off
    break;

    case CYAN:
      PORTB &= ~((1<<PB6)|(1<<PB7)); //G & B on
      PORTB |= (1<<PB5); //R off
      
    break;

    case MAGENTA:
      PORTB &= ~((1<<PB5)|(1<<PB7)); //R & B on
      PORTB |= (1<<PB6); //G off
    break;

    case YELLOW:
    PORTB &= ~((1<<PB5)|(1<<PB6)); //R & G on
      PORTB |= (1<<PB7); //B off
    break;
  }
}

//****************************************
//SET ACTION******************************
//****************************************
void ActOnRobotState()
{
  switch (CurrentRobotDirection)
  {
    case Forward:
      fwd(Speed*1.5);
      CurrentRGBColour = GREEN;
    break;
    
    case Left:
      bak(Speed*1.5);
      delay(500);
      rgt(Speed*2);
      delay(100);
      CurrentRGBColour = YELLOW;
    break;
    
    case Right:
      bak(Speed*1.5);
      delay(500);
      lft(Speed*2);
      delay(100);
      CurrentRGBColour = MAGENTA;
    break;
    
    case Stop:
      halt();
      CurrentRGBColour = RED;
    break;

    case Back:
      bak(Speed*1.5);
      delay(500);
      rgt(Speed);
      delay(500);
      CurrentRGBColour = BLUE;
    break;
  }
}



//****************************************
//WORK OUT THE ROBOT STATE****************
//****************************************
void CalculateRobotState(unsigned char LeftSensor, unsigned char RightSensor)
{
  if ((LeftSensor == 0) && (RightSensor == 0))
    {
      CurrentRobotDirection = Forward;
    }
  if ((LeftSensor == 0) && (RightSensor == 1))
    {
      CurrentRobotDirection = Right;
    }
  if ((LeftSensor == 1) && (RightSensor == 0))
    {
      CurrentRobotDirection = Left;
    }
  if ((LeftSensor == 1) && (RightSensor == 1))
    {
      CurrentRobotDirection = Back;
    } 
    Serial.println(CurrentRobotDirection);
}



//****************************************
//READ GROUND SENSORS*********************
//****************************************
void ReadGroundLED(unsigned char LEDIndex)
{
  TurnGroundLEDOn(LEDIndex);
  delay(10);
  RawSensorReading = analogRead((LEDIndex+8)); //ADC 9 and 10 are the IR receivers
  
  TurnGroundLEDOff(LEDIndex);

  if (RawSensorReading >= Threshold)
  {
    SensorStates[LEDIndex] = 1;
  }
  else
  {
    SensorStates[LEDIndex] = 0;
  }
  Serial.print("Sensor ");
  Serial.print(LEDIndex);
  Serial.print(" = ");
  Serial.print(RawSensorReading);
  if (SensorStates[LEDIndex] == 1)
  {
    Serial.print(" BLACK  ");
  }
  else
  {
    Serial.print(" WHITE  ");
  }
}

//****************************************
//Ground Sensor LEDs - active low ********
//****************************************
//Ground Sensor LED OFF *******************
void TurnGroundLEDOff(unsigned char LEDIndex)
{
  if ((LEDIndex ==1) || (LEDIndex ==2)) {
    PORTJ |= (1<<LEDIndex); 
  } 
}

//Ground Sensor LED On *******************
void TurnGroundLEDOn(unsigned char LEDIndex)
{
  if ((LEDIndex ==1) || (LEDIndex ==2))
  {
    PORTJ &= ~(1<<LEDIndex); 
  }
   
}
//****************************************
//GROUND SENSORS END**********************
//****************************************


//****************************************
//DRIVE DEFINITIONS***********************
//****************************************
void fwd(int Speed){
  leftMotorForward(Speed);
  rightMotorForward(Speed);
}
  
//****************************************
void bak(int Speed){
  leftMotorBackward(Speed);
  rightMotorBackward(Speed);
}
//****************************************
void rgt(int Speed){
  leftMotorForward(Speed);
  rightMotorBackward(Speed);
}
//****************************************
void lft(int Speed){
  leftMotorBackward(Speed);
  rightMotorForward(Speed);
}
//****************************************
void halt(){
  leftMotorStop();
  rightMotorStop();
}



//****************************************
void leftMotorForward(float Speed)
{
  analogWrite(7,0);
  analogWrite(6,Speed*2);
}

//****************************************
void leftMotorBackward(float Speed)
{
  analogWrite(7,Speed*2);
  analogWrite(6,0);
}
//****************************************
void leftMotorStop()
{
  analogWrite(7,0);
  analogWrite(6,0);
}

//****************************************
void rightMotorForward(float Speed)
{
    analogWrite(5,Speed*2.5);
    analogWrite(2,0);
}

//****************************************
void rightMotorBackward(float Speed)
{
    analogWrite(5,0);
    analogWrite(2,Speed*2);
}
//****************************************
void rightMotorStop()
{
    analogWrite(2,0);
    analogWrite(5,0); 
}

See https://www.gctronic.com/doc/index.php/Elisa-3 for more information about this robot.