LineFollowingWithMelody.ino

/* Created by BrothEr yUnN
 *  Combination MakerUNO & LSS05 & Motor & Colour Sensor
 *  7/3/2018 setup for Line Following Robot with Melody
 *  
 *  Connection wire motor to Motor Driver
 *  A = Black wire
 *  B = White wire
 *  
 *  Connection wire Colour Sensor to Arduino
 *  VCC = 5V
 *  GND = Ground
 *  S0  = pin 12
 *  S1  = pin 11
 *  S2  = pin 9
 *  S3  = pin 10
 *  OUT = pin 13
 */

/*************************************************
* Public Constants
*************************************************/
#define NOTE_B0 31
#define NOTE_C1 33
#define NOTE_CS1 35
#define NOTE_D1 37
#define NOTE_DS1 39
#define NOTE_E1 41
#define NOTE_F1 44
#define NOTE_FS1 46
#define NOTE_G1 49
#define NOTE_GS1 52
#define NOTE_A1 55
#define NOTE_AS1 58
#define NOTE_B1 62
#define NOTE_C2 65
#define NOTE_CS2 69
#define NOTE_D2 73
#define NOTE_DS2 78
#define NOTE_E2 82
#define NOTE_F2 87
#define NOTE_FS2 93
#define NOTE_G2 98
#define NOTE_GS2 104
#define NOTE_A2 110
#define NOTE_AS2 117
#define NOTE_B2 123
#define NOTE_C3 131
#define NOTE_CS3 139
#define NOTE_D3 147
#define NOTE_DS3 156
#define NOTE_E3 165
#define NOTE_F3 175
#define NOTE_FS3 185
#define NOTE_G3 196
#define NOTE_GS3 208
#define NOTE_A3 220
#define NOTE_AS3 233
#define NOTE_B3 247
#define NOTE_C4 262
#define NOTE_CS4 277
#define NOTE_D4 294
#define NOTE_DS4 311
#define NOTE_E4 330
#define NOTE_F4 349
#define NOTE_FS4 370
#define NOTE_G4 392
#define NOTE_GS4 415
#define NOTE_A4 440
#define NOTE_AS4 466
#define NOTE_B4 494
#define NOTE_C5 523
#define NOTE_CS5 554
#define NOTE_D5 587
#define NOTE_DS5 622
#define NOTE_E5 659
#define NOTE_F5 698
#define NOTE_FS5 740
#define NOTE_G5 784
#define NOTE_GS5 831
#define NOTE_A5 880
#define NOTE_AS5 932
#define NOTE_B5 988
#define NOTE_C6 1047
#define NOTE_CS6 1109
#define NOTE_D6 1175
#define NOTE_DS6 1245
#define NOTE_E6 1319
#define NOTE_F6 1397
#define NOTE_FS6 1480
#define NOTE_G6 1568
#define NOTE_GS6 1661
#define NOTE_A6 1760
#define NOTE_AS6 1865
#define NOTE_B6 1976
#define NOTE_C7 2093
#define NOTE_CS7 2217
#define NOTE_D7 2349
#define NOTE_DS7 2489
#define NOTE_E7 2637
#define NOTE_F7 2794
#define NOTE_FS7 2960
#define NOTE_G7 3136
#define NOTE_GS7 3322
#define NOTE_A7 3520
#define NOTE_AS7 3729
#define NOTE_B7 3951
#define NOTE_C8 4186
#define NOTE_CS8 4435
#define NOTE_D8 4699
#define NOTE_DS8 4978

#define CHOICE_RED  (1 << 0)
#define CHOICE_GREEN    (1 << 1)
#define CHOICE_BLUE (1 << 2)
#define CHOICE_YELLOW   (1 << 3)

// Buzzer pin definitions
#define BUZZER1  8//4

#define S0 12
#define S1 11
#define S2 9
#define S3 10
#define sensorOut 13

// Stores frequency read by the photodiodes
int redFrequency = 0;
int greenFrequency = 0;
int blueFrequency = 0;

int redColor = 0;
int greenColor = 0;
int blueColor = 0;

//---------------------------------------------------------------------------//
//Assign Pins For Motor Driver (Fixed for this component)
int EN1 = 5;    //Left Enable Motor
int DIR1= 4;    //Left Direction Motor

int EN2 = 6;    //Right Enable Motor
int DIR2= 7;    //Right Direction Motor

int LeftSen   = A1;      //Left Sensor
int LeftMSen  = A2;      //Left Middle Sensor
int MidSen    = A3;      //Middle Sensor
int RightMSen = A4;      //Right Middle Sensor
int RightSen  = A5;      //Right Sensor

int IR_Left   = 0;   //OUT1
int IR_MLeft  = 0;   //OUT2
int IR_Middle = 0;   //OUT3
int IR_MRight = 0;   //OUT4
int IR_Right  = 0;   //OUT5

//Motor Reading
int Motor_Left  = 0;
int Motor_Right = 0;
   
void setup()
{
  // Setting the outputs
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  
  pinMode(BUZZER1, OUTPUT);
  
  // Setting the sensorOut as an input
  pinMode(sensorOut, INPUT);

/* 
 * Frequency Scaling :
 * Power Down : S0 = LOW  , S1 = LOW
 *      2%    : S0 = LOW  , S1 = HIGH
 *      20%   : S0 = HIGH , S1 = LOW
 *      100%  : S0 = HIGH , S1 = HIGH
 * For the Arduino, it is common to use a frequency scaling of 20%.
 */
 
  // Setting frequency scaling to 20%
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);

  //Motor Driver Pin Setup
  pinMode ( EN1, OUTPUT);
  pinMode (DIR1, OUTPUT);
  pinMode ( EN2, OUTPUT);
  pinMode (DIR2, OUTPUT);

  //For motor direction moving forward
  digitalWrite(DIR1, 1);
  digitalWrite(DIR2, 0);
  
  //LSS05 Auto-Calibrating Line Sensor Pin Setup
  pinMode(LeftSen,  INPUT);
  pinMode(LeftMSen, INPUT);
  pinMode(MidSen,   INPUT);
  pinMode(RightMSen,INPUT);
  pinMode(RightSen, INPUT);
  
  Serial.begin(9600);
  delay(10);
}

void readSensor ()
{
  IR_Left   = digitalRead(LeftSen);      //OUT1
  IR_MLeft  = digitalRead(LeftMSen);     //OUT2
  IR_Middle = digitalRead(MidSen);       //OUT3
  IR_MRight = digitalRead(RightMSen);    //OUT4
  IR_Right  = digitalRead(RightSen);     //OUT5

  Serial.print("SL = ");
  Serial.print(IR_Left );
  Serial.print("\t");
  Serial.print("SML = ");
  Serial.print(IR_MLeft);
  Serial.print("\t");
  Serial.print("SM = ");
  Serial.print(IR_Middle);
  Serial.print("\t");
  Serial.print("SMR = ");
  Serial.print(IR_MRight);
  Serial.print("\t");
  Serial.print("SR = ");
  Serial.print(IR_Right);

  Serial.println();
}

void ColourMelody ()
{
  // Setting RED (R) filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,LOW);
  
  // Reading the output frequency
  redFrequency = pulseIn(sensorOut, LOW);
  redColor = map(redFrequency, 35,130,255,0);
  
  // Printing the RED (R) value
  Serial.print("R = ");
  //Serial.print(redFrequency);     //To see the frequency of colour
  Serial.print(redColor);
  delay(50);

  // Setting GREEN (G) filtered photodiodes to be read
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  
  // Reading the output frequency
  greenFrequency = pulseIn(sensorOut, LOW);
  greenColor = map(greenFrequency,  48, 140, 255, 0);

  // Printing the GREEN (G) value 
  Serial.print(" G = ");
  //Serial.print(greenFrequency);   //To see the frequency of colour
  Serial.print(greenColor);
  delay(50);

  // Setting BLUE (B) filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  
  // Reading the output frequency
  blueFrequency = pulseIn(sensorOut, LOW);
  blueColor = map(blueFrequency,  37, 110, 255, 0);

  // Printing the BLUE (B) value 
  Serial.print(" B = ");
  //Serial.println(blueFrequency);  //To see the frequency of colour
  Serial.print(blueColor);
  delay(50);

  if (redColor > greenColor && redColor > blueColor){
    Serial.println(" - RED detected!");
    toner(CHOICE_RED, 150);
  }
  if (greenColor > redColor && greenColor > blueColor){
    Serial.println(" - GREEN detected!");
    toner(CHOICE_GREEN, 150);
  }
  if (blueColor > redColor && blueColor > greenColor){
    Serial.println(" - BLUE detected!");
    toner(CHOICE_BLUE, 150);
  }
}

void buzz_sound(int buzz_length_ms, int buzz_delay_us)
{
  // Convert total play time from milliseconds to microseconds
  long buzz_length_us = buzz_length_ms * (long)500;

  // Loop until the remaining play time is less than a single buzz_delay_us
  while (buzz_length_us > (buzz_delay_us * 2))
  {
    buzz_length_us -= buzz_delay_us * 2; //Decrease the remaining play time

    // Toggle the buzzer
    digitalWrite(BUZZER1, LOW);
    delayMicroseconds(buzz_delay_us);

    digitalWrite(BUZZER1, HIGH);
    delayMicroseconds(buzz_delay_us);
  }
}

/*
 Light an LED and play tone
 Red, upper left:     440Hz - 2.272ms - 1.136ms pulse
 Green, upper right:  880Hz - 1.136ms - 0.568ms pulse
 Blue, lower left:    587.33Hz - 1.702ms - 0.851ms pulse
 Yellow, lower right: 784Hz - 1.276ms - 0.638ms pulse
*/
 
void toner(byte which, int buzz_length_ms)
{
  //Play the sound
  switch(which) {
  case CHOICE_RED:
    buzz_sound(buzz_length_ms, 1136); 
    break;
  case CHOICE_GREEN:
    buzz_sound(buzz_length_ms, 568); 
    break;
  case CHOICE_BLUE:
    buzz_sound(buzz_length_ms, 851); 
    break;
  case CHOICE_YELLOW:
    buzz_sound(buzz_length_ms, 638); 
    break;
  }
}

void loop()
{
  readSensor();
  ColourMelody();
  delay(1);
  
  if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==1)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 150);   //PWM Speed Control
    analogWrite(EN1, 150);   //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==1)&&(digitalRead(RightMSen)==1)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 150);   //PWM Speed Control
    analogWrite(EN1, 140);   //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==1)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 150);   //PWM Speed Control
    analogWrite(EN1, 90);    //PWM Speed Control
  } 
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==1)&&(digitalRead(RightSen)==1))
  {
    analogWrite(EN2, 150);   //PWM Speed Control
    analogWrite(EN1, 50);    //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==1))
  {
    analogWrite(EN2, 150);   //PWM Speed Control
    analogWrite(EN1, 0);     //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==1)&&(digitalRead(MidSen)==1)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 140);   //PWM Speed Control
    analogWrite(EN1, 150);   //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==0)&&(digitalRead(LeftMSen)==1)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 90);    //PWM Speed Control
    analogWrite(EN1, 150);   //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==1)&&(digitalRead(LeftMSen)==1)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 50);    //PWM Speed Control
    analogWrite(EN1, 150);   //PWM Speed Control
  }
  else if((digitalRead(LeftSen)==1)&&(digitalRead(LeftMSen)==0)&&(digitalRead(MidSen)==0)&&(digitalRead(RightMSen)==0)&&(digitalRead(RightSen)==0))
  {
    analogWrite(EN2, 0);     //PWM Speed Control
    analogWrite(EN1, 150);   //PWM Speed Control
  }
  else
  { 
    analogWrite(EN2, 80);   //PWM Speed Control
    analogWrite(EN1, 80);   //PWM Speed Control
  }
}