Close

Frobo V2 Programming Lesson- Part 7

We hope that you have understood how to read sensors and control some output devices with Froboduino/Arduino programming.

This is now time for you to jump to the next level of professional programming methods. Now, your Frobo is connected with an Obstacle sensor, IR colour contrast detection module, light sensing LDRs, Motors, Servo and Led lights. For now, this is enough to make Frobo perform a few composite tasks. Let us define the functions that we want Frobo to perform. You can come up with your own functionalities later and develop code.

We will use all three sensor inputs and all output devices attached. The next code will enable Frobo with the following capabilities.

1-      When light goes below a certain level, Frobo will stop moving and its eyes will blink with slow rate.

2-      When light is normal (normal??, we define what is normal in our code by minimum LDR value), Frobo will move inside a circle drawn with black electrical tape or marker on the floor.

3-      When Frobo senses the boundary, it moves away from the line. There are three conditions, in which Frobo wil approach the line. A)- When it approaches the line at an angle from left side. B)- When it approaches the line at an angle from right side. C)- When it approaches the line straight.

3-      If Frobo detects obstacles from 3000 cms, Frobo creates a sound  and approaches the object and and stops when distance is 300 cms. Try coming in fron of Frobo at a distance of 3000 cms( 3 meter) or less. Start walking away and Frobo will follow you.

4-      Frobo will blink eyes and create sounds in different patterns to various sensor inputs.

So, let's get coding. Please read comments after "//" or between  "*/   /*" to understand the part of the code.

 #include //declare inputs and output devices.const int trigPin = 2;const int echoPin = 3;const int LD1 = A4;const int LD2 = A5;const int IRD1 = A6;const int IRD2 = A7;const int leftLed = 9;const int rightLed = 10;const int accelerator = 5;const int leftMotorFwd = A0;const int leftMotorRev = A1;const int rightMotorFwd = A2;const int rightMotorRev = A3;const int buzzer = 0;Servo steeringServo;//Declare variables we are going to monitor.int servoPos = 90;int ldrValue1 = 0;int ldrValue2 = 0;int lightMode = 1;//Define vaiables left and right states as boolean ( 0 or 1 state) reading of IR diodes.bool leftState = 0;bool rightState =0;// Declare left and right Led States for controlling Leds.int leftLedState = 0;int rightLedState = 0;int ledInterval = 500;//We declare another set of variables to decide if Frobo moved over the boundary line.int lineDetect =0;// This one if Frobo dtected line on left side.int lineDetectLeft = 0;//This one if Frobo dtected line on right side.int lineDetectRight = 0;//Her ewe declare detect time variables as unsigned long types.unsigned long detectStartTime = 0;//We will define these variables in the code.unsigned long detectNowTime = 0;unsigned long detectOnTime = 0;//Declare Led start time.unsigned long ledStartTime = 0;unsigned long ledNowTime = 0;unsigned long ledOnTime = 0; //We need not define pinMode as all pins are taken as INPUT by default. Setup code runs only once when program starts. void setup() { // Define buzeer, LEDs, Motors and Servo as outputs. Inputs are defined by default.pinMode(trigPin, OUTPUT);pinMode(leftLed, OUTPUT);pinMode(rightLed, OUTPUT);pinMode(accelerator, OUTPUT);pinMode(leftMotorFwd, OUTPUT);pinMode(leftMotorRev, OUTPUT);pinMode(rightMotorFwd, OUTPUT);pinMode(rightMotorRev, OUTPUT);pinMode(buzzer, OUTPUT);// Let us open the serial ports with data rate 9600 bits /secondsteeringServo.attach(6);Serial.begin(9600); } //This loop code runs repeatedly void loop() { // This code runs repeatedly. //Read both LDR values.int ldrValue1 = analogRead(A4);int ldrValue2 = analogRead(A5);/* Note down LDR values in different directions when light is on and repeat the same by turning off the light.LDR values go up when light is low and lower when there is more light. we will use the highest value when light is on as a threshold value to make Frobo to sense when light is on and when it is off. */ Serial.print("Left LDR value ");Serial.println(ldrValue1);Serial.print("Right LDR value ");Serial.println(ldrValue2);//Remove "//" from next line when taking LDR values.so you can record LDR values in Serial monitor for easy reading.//delay(1000);/*When light is off. We will see if ldrValues are higher than the minimum we recorded. * Frobo will move only when lightMode =1. * We are taking 300 here. Replace it with your recorded minimum. We will change int lightMode to 0. */if (ldrValue1 <= 300 && ldrValue2 <= 300){ lightMode = 0;}//Program will run the below modules repeatedly.// We write these programs in different modules separately for easier programming and debugging. myBoundary(); ledPattern1(); ledPattern2(); ledPattern3(); ledPattern4(); } // The code below is to program Frobo to remain within the boundary drawn.void myBoundary(){ //Read IR diode state to check if Frobo sees the boundary.leftState = digitalRead(IRD1);rightState = digitalRead(IRD2);// If there is enough light.if (lightMode == 1){ // Frobo keeps moving forward. Both motors move forward. digitalWrite(accelerator, HIGH); digitalWrite(leftMotorFwd, HIGH); digitalWrite (leftMotorRev, LOW); digitalWrite(rightMotorFwd, HIGH); digitalWrite (rightMotorRev, LOW); // If both IR diodes sees line, move Frobo backwards and then turn left (or even right as you like but change code for right turn).if (IRD1 == 0 || IRD2 == 0){ if(lineDetect == 0){ //Take time reading when line was detected. detectStartTime = millis(); // Turn left motor backwards. digitalWrite(leftMotorFwd, LOW); digitalWrite (leftMotorRev, HIGH);// Turn right motor backwards. digitalWrite(rightMotorFwd, LOW); digitalWrite (rightMotorRev, HIGH); //Line found. So, we change the lineDetect to 1. lineDetect = 1; // We close the statement here as lineDetect state has changed.}}if (lineDetect == 1){ // Take current time since detection. detectNowTime = millis(); // Declare detectOnTime as time difference since line was detected. detectOnTime = detectNowTime - detectStartTime; // Now we want to Frobo to move backwards for 5 seconds (or you can decide) and then move forward while turning left. if (detectOnTime >= 5000){ // Turn left motor forward. servoPos = servoPos + 15; steeringServo.write(servoPos); digitalWrite(leftMotorFwd, HIGH); digitalWrite (leftMotorRev, LOW);// Turn right motor forward. digitalWrite(rightMotorFwd, HIGH); digitalWrite (rightMotorRev, LOW); } // Turn servo back by 15 degrees after 3 seconds more. if (detectOnTime >=8000){ servoPos = servoPos - 15; steeringServo.write(servoPos); detectNowTime = 0; detectStartTime = detectNowTime; lineDetect =0; }} // If right IR diode sees line, but left doesn't. Turn Servo left by 15 degrees for 5 seconds.if (IRD1 == 1 || IRD2 == 0){ servoPos = servoPos - 15; steeringServo.write(servoPos); detectStartTime = millis(); lineDetectLeft = 1;}if (lineDetectLeft == 1){ detectNowTime = millis(); detectOnTime = detectNowTime - detectStartTime; if (detectOnTime >= 5000); //Turn servo back by 15 degrees after 5 seconds. servoPos = servoPos +15; steeringServo.write(servoPos); detectNowTime = 0; detectStartTime = detectNowTime; lineDetectLeft = 0; } // If left IR diode sees line, but right doesn't. Turn Servo right by 15 degrees for 5 seconds.if (IRD1 == 0 || IRD2 == 1){ servoPos = servoPos + 15; steeringServo.write(servoPos); detectStartTime = millis(); lineDetectRight = 1;}if (lineDetectRight == 1){ detectNowTime = millis(); detectOnTime = detectNowTime - detectStartTime; if (detectOnTime >= 5000); //Turn servo back by 15 degrees after 5 seconds. servoPos = servoPos -15; steeringServo.write(servoPos); detectNowTime = 0; detectStartTime = detectNowTime; lineDetectRight = 0; }}}// This code will control Leds in a way so that Leds are On for 1.5 seconds and off for 100 mili seconds. Showing fear.void ledPattern1(){ //This code will run only when light is normal and both IR diodes step on boundary. if (lightMode == 1 && lineDetect == 1){ //define ledOnTime. ledOnTime = ledNowTime - ledStartTime; if (leftLedState == 0 && rightLedState == 0){ ledStartTime = millis();digitalWrite(leftLed, HIGH);digitalWrite(rightLed, HIGH);//We use tone(pin, frequency) function to sound buzzer. Our buzzer is at pin 0 and frequency we have chsen here is 440 Hz.tone(0, 440);leftLedState = 1;rightLedState = 1; } // This code lights up Leds for 1500 mili seconds (1.5 seconds) if (leftLedState == 1 && rightLedState ==1){ ledNowTime = millis(); if (ledOnTime >= 1500){ digitalWrite(leftLed, LOW);digitalWrite(rightLed, LOW);//Here we use noTone(pin) to silence the buzzer.noTone(0); leftLedState = 2; rightLedState = 2; } } if (leftLedState == 2 && rightLedState == 2){ if (ledOnTime >= 1600){ ledNowTime = ledStartTime; leftLedState = 0; rightLedState = 0; } } }}//This code will control Leds when light is normal and when Frobo sees line on the left.void ledPattern2(){ if (lightMode == 1 && lineDetectLeft == 1){ //We will light up right Led in this mode and blibk left. digitalWrite(rightLed, HIGH); //define ledOnTime. ledOnTime = ledNowTime - ledStartTime; if (leftLedState == 0){ ledStartTime = millis();digitalWrite(leftLed, HIGH);tone(0, 330);leftLedState = 1; } // This code lights up left Led for 1500 mili seconds (1.5 seconds) if (leftLedState == 1 ){ ledNowTime = millis(); if (ledOnTime >= 1500){ digitalWrite(leftLed, LOW); noTone(0); leftLedState = 2; } } if (leftLedState == 2 ){ if (ledOnTime >= 1600){ ledNowTime = ledStartTime; leftLedState = 0; } } } } // This code runs whenFrobo see line on the right IR diode.void ledPattern3(){ if (lightMode == 1 && lineDetectRight == 1){ //We will light up left Led in this mode and blink right. digitalWrite(leftLed, HIGH); //define ledOnTime. ledOnTime = ledNowTime - ledStartTime; if (rightLedState == 0){ ledStartTime = millis();digitalWrite(rightLed, HIGH);tone(0, 330);rightLedState = 1; } // This code lights up left Led for 1500 mili seconds (1.5 seconds) if (leftLedState == 1 ){ ledNowTime = millis(); if (ledOnTime >= 1500){ digitalWrite(rightLed, LOW); noTone(0); rightLedState = 2; } } if (rightLedState == 2 ){ if (ledOnTime >= 1600){ ledNowTime = ledStartTime; rightLedState = 0; } } } } // This code runs when light level goes below normal.void ledPattern4(){ if (lightMode == 0){ //define ledOnTime. ledOnTime = ledNowTime - ledStartTime; if (leftLedState == 0&& rightLedState == 0){ ledStartTime = millis();digitalWrite(leftLed, HIGH);digitalWrite(rightLed, HIGH); tone(0, 31); leftLedState = 1;rightLedState = 1; } // This code lights up both Led for 100 mili seconds. if (leftLedState == 1 && rightLedState == 1 ){ ledNowTime = millis(); if (ledOnTime >= 100){ digitalWrite(leftLed, LOW);digitalWrite(rightLed, LOW);noTone(0); // Let us sound the buzzer using tone() function. leftLedState = 2; } } //THis code keep Leds off for 80 mili seconds. if (leftLedState == 2 && rightLedState == 2){ if (ledOnTime >= 180){ digitalWrite(leftLed, HIGH);digitalWrite(rightLed, HIGH);tone(0, 31); leftLedState = 3; rightLedState == 3; } } //Keep Leds on for 100 ms. if (leftLedState == 3 && rightLedState == 3 ){ if (ledOnTime >= 280){ digitalWrite(leftLed, LOW);digitalWrite(rightLed, LOW);noTone(0); leftLedState = 4; rightLedState == 4; } } //Keeps Leds off for 500 ms to create a Heartbeat kind of blinking effect. if (leftLedState == 4 && rightLedState == 4 ){ if (ledOnTime >= 780){ digitalWrite(leftLed, HIGH);digitalWrite(rightLed, HIGH);tone(0, 31); leftLedState = 5; rightLedState == 5; } } if (leftLedState == 5 && rightLedState == 5 ){ if (ledOnTime >= 880){ digitalWrite(leftLed, LOW);digitalWrite(rightLed, LOW);noTone(0); leftLedState = 6; rightLedState == 6; } } if (leftLedState == 6 && rightLedState == 6 ){ if (ledOnTime >= 1000){ ledNowTime = ledStartTime; leftLedState = 0; rightLedState == 0; } } } }

Copy and pastre this code in IDE dialog box and compile for any errors. Take note of LDR values  and add the minimum value when Light is normal you note and change it accordingly in the code. Please read the code carefully. Please ask questions in "Comments" section below.