7. Program Explanation

Share for us
Share on facebook
Share on twitter
Share on pinterest
Share on whatsapp

Through the explanation previously, you should already have a general understanding of the robot. Next we will come to the most important part – theory and program. Most of the programs provided are compiled as libraries and you just need to call them in the Arduino IDE. So in the following let’s focus on how to compile them.

7.1 Smiling Face with LEDs

The smiling face of the robot is displayed with 16 yellow LEDs. They are controlled directly by the 74HC595 chip. In the code, change the parameter in the output function Dataout() to 0xff, so the LEDs forming a smiling face will light up. Change it to 0x00, and the LEDs will go out.

/*****************Output of data control********************/
void RollbotLED::DataOut(int val)
{
  for (int i = 0;i < 8;i++)
  {
      if(val&0x80)
        digitalWrite(dataPin,1);
      else
        digitalWrite(dataPin,0); 
      val<<=1;
      digitalWrite(clockPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(clockPin, LOW);   
  }
  digitalWrite(latchPin,LOW);
  delayMicroseconds(10);
  digitalWrite(latchPin,HIGH);
}

7.2 Sensor Signal Intensity on OLED Screen

The OLED screen can display characters, letters, and patterns. It is used in the robot for your better interaction with the robot. How to use it? Here IIC is used in hardware. First, initialize the OLED screen. For use, two functions are indispensable: WriteCommand() and WriteData().

/********************OLED command writing*******************/
void RollbotOLED::WriteCommand(unsigned int ins)
{
  Wire.beginTransmission(0x78 >> 1);//0x78 >> 1
  Wire.write(0x00);//0x00
  Wire.write(ins);
  Wire.endTransmission();
}
/*********OLED data writing***********/
void RollbotOLED::WriteData(unsigned int dat)
{
  Wire.beginTransmission(0x78 >> 1);//0x78 >> 1
  Wire.write(0x40);//0x40
  Wire.write(dat);
  Wire.endTransmission();
}

Next, the function for positions on the OLED screen: SetPos(). With this function, you can locate the specific point of the OLED display. 

void RollbotOLED::IIC_SetPos(unsigned int x, unsigned int y)
{
  WriteCommand(0xb0 + y);
  WriteCommand(((x & 0xf0) >> 4) | 0x10); //|0x10
  WriteCommand((x & 0x0f) | 0x00); //|0x01
}

Apart from the three basic functions, you can use more for display on the OLED screen. The current code is adequate for displaying sensor signal intensity easily. For more information, you can refer to the program RollbotOLED.CPP in the library.

7.3 Line Following

The line following function is the most important function of the robot. It distinguishes the black and white surfaces through the infrared sensor module and applies the PD algorithm to realize line following. It collects and saves the AD value of the 5 pins: A0, A1, A2, A3, and A7.

/*********Read data of the sensors***********/
void RollbotReadSensor::Read_Data()
{
  data[0] = analogRead(A0);              
  data[1] = analogRead(A1);
  data[2] = analogRead(A2);
  data[3] = analogRead(A3);
  data[4] = analogRead(A7);                
  OLED_Flag[0] = map(data[0], 50, 500, 3, 6);   
  OLED_Flag[1] = map(data[1], 50, 500, 3, 6);
  OLED_Flag[2] = map(data[2], 50, 500, 3, 6);
  OLED_Flag[3] = map(data[3], 50, 500, 3, 6);
  OLED_Flag[4] = map(data[4], 50, 500, 3, 6);   
}

Compare the collected AD value with the given value and generate a result which indicates the position of the robot to the black line, as shown below.

int RollbotReadSensor::Read_BlackFlag()
{
    while(1)
    {
      Read_Data();
    if ((data[0] < threshold) && (data1] > threshold) && (data[2] > threshold) && (data[3] > threshold) && (data[4] > threshold)) return -4;
      else if ((data[0] < threshold) && (data[1] < threshold) && (data[2] > threshold) && (data[3] > threshold) && (data[4] > threshold)) return -3;
      else if ((data[0] > threshold) && (data[1] < threshold) && (data[2] > threshold) && (data[3] > threshold) && (data[4] > threshold)) return -2;
      else if ((data[0] > threshold) && (data[1] < threshold) && (data[2] < threshold) && (data[3] > threshold) && (data[4] > threshold)) return -1;   
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] < threshold) && (data[3] > threshold) && (data[4] > threshold)) return 0;    
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] < threshold) && (data[3] < threshold) && (data[4] > threshold)) return 1; 
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] > threshold) && (data[3] < threshold) && (data[4] > threshold)) return 2;
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] > threshold) && (data[3] < threshold) && (data[4] < threshold)) return 3;
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] > threshold) && (data[3] > threshold) && (data[4] < threshold)) return 4;
      else if ((data[0] < threshold) && (data[1] < threshold) && (data[2] < threshold) && (data[3] < threshold)) return 5;
      else if ((data[1] < threshold) && (data[2] < threshold) && (data[3] < threshold) && (data[4] < threshold)) return 5;
      else if ((data[0] > threshold) && (data[1] > threshold) && (data[2] > threshold) && (data[3] > threshold) && (data[4] > threshold)) return 6;
    }
}

Finally, we just need to read the position of the robot and adjust the related parameters via the PD formula below, thus realizing the function of following lines.

MotorSpeed = P * SignalValue + D * (SignalValue – LastError)

SignalValue is the gap between the position of RollbotMicro detected and that of the black line. LastError is the error value accumulated till the last time. SignalValue – LastError means the rate of change of the relative position.

Motorpeed = int(P * SignalValue + D * (SignalValue - LastError));
    LastError = 0;
    LastError = SignalValue;
    Speed_L = BASE_SPEED + Motorpeed;
    Speed_R = BASE_SPEED - Motorpeed;
    if (Speed_R > MAX_SPEED ) Speed_R = MAX_SPEED;
    if (Speed_L > MAX_SPEED )  Speed_L = MAX_SPEED;
    if (Speed_R < MIN_SPEED) Speed_R = MIN_SPEED;
    if (Speed_L < MIN_SPEED)   Speed_L = MIN_SPEED;
    Motor.Motordrive(Speed_Dir, Speed_L, Speed_R);