to mark as completed Flok-0-Bot
Flok-0-Bot is an old robot I have made .What it does is to follow light sources and avoid any obstacle on its way to the light source
and when it finds it then it does some sort of dance though I dont have a video and cant shoot one cause I have dissassembled it
to use its parts on other projects now I only have the chassis .
But you can see couple of photos .
What I have used ?
An arduino duemilanove , 2 standard parallax futaba cr servos , 4 ldrs and a sharp sensor dont remember the actuall part number
and ofcourse pieces of aluminium sheet that I have processed and made this kind of shape .
You can also see my mums laundry on the back . :P
The code I wrote with the help of a friend follows :
/*
* Flokos Light Follower Rover
* code by Orfeus and Flokos for GRobot.gr
* HER Extra 200912202045
*/
const int maxLightThreshold = 500; // Max Light Strength Threshold
const int lightThreshold = 50; // Light Strength Threshold
int ldrPins[] = {0, 1, 2, 3}; // We set the LDRs in an array as : 0=Front, 1=Right, 2=Back, 3= Left
int irSensorPin = 4; // Attach Sharp IR Sensor to Analog pin 4
int runButton = 10; // Start / Stop Button pin
int ledPin = 13; // LED Pin
int minObstacleDistance = 400; // Set the minimum distance of an obstacle
int curLight = 0; // Variable to keep the light strength
int measurment = 10; // How many measurments to take
int servoPinLeft = 5; // Digital pin for left CR Servo
int servoPinRight = 6; // Digital pin for right CR Servo
int servoCenter = 1500; // Servos zero point. Change this to your servo setting
// You must have two identicall servos
int minServoPulse = servoCenter - 500; // minimum servo position
int maxServoPulse = servoCenter + 500; // maximum servo position
int refreshTime = 20; // time (ms) between pulses (50Hz)
long lastPulse = 0; // recorded time (ms) of the last pulse
int firstRun = 1; // If this is the first time prg runs
int buttonState = 1; // Keep the button state,If you don't want to use a button change this to "1"
int useButton = 1; // If you don't want to use a button change this to "0"
int debug = 0; // Debug prints infos on serial port [0=none, 1=min, 2=max]
void setup()
{
/* for (int n = 0 ; n < 4 ; n++)
{
pinMode(ldrPins[n], INPUT); // Set all LDR Pins for Input !!!!! No need
}
*/
pinMode(irSensorPin, INPUT); // Set the IR pin for input
pinMode(runButton, INPUT); // Set the button pin as input
pinMode(ledPin, OUTPUT); // Set the button pin as output
pinMode(servoPinLeft,OUTPUT); // Set the servos pin
pinMode(servoPinRight,OUTPUT);
if (debug) Serial.begin(9600); // Just for debuging
}
void loop()
{
if (useButton == 1)
{
attachInterrupt(0, readButton, FALLING);
}
if (debug)
{
Serial.println("*************************************************");
Serial.println("* Program START by Orfeus anf Flokos 20091220 *");
Serial.println("*************************************************");
}
if (firstRun)
{
doDance(8000);
firstRun = 0;
}
while (noObstacle()) // Do all that only while no obstacle ahead
{
if (buttonState)
{
int curDirection = checkLight(); // Get the Higher Light Direction
turnToLight(curDirection); // Turn Rover to Light
while (curLight < maxLightThreshold && noObstacle() && buttonState) // If light is not stronger than threshold
{
curLight = analogRead(ldrPins[0]);
move(0); // Use the Move Function to that direction
}
if (curLight >= maxLightThreshold) // If light is stronger
{
if (debug) Serial.println("We found the Liht !!!!!!!!!!!!!!");
doDance(8000); // Dance for me
moveRandomBack(3000);
}
curLight = 0;
}
}
if (debug) Serial.println("Obstacle Found !!!!! Go Back !!!!!");
moveRandomBack(2000);
}
/************************
* Functions Start *
*************************/
void readButton (void)
{
if (digitalRead(runButton) == LOW)
{
buttonState = !buttonState;
}
if (debug)
{
Serial.print("Button:");
Serial.println(buttonState);
}
}
/*
* Function to turn the rover to Max Light Direction
*/
void turnToLight(int turnDirection)
{
int newFooLight;
int fooLight = analogRead(ldrPins[turnDirection]); // Read the ligh from maxlight sensor
if (turnDirection == 2) turnDirection = 1;
if (debug){
Serial.print("We are turning to the light: ");
Serial.println(turnDirection);
}
do
{
move(turnDirection); // Move to direction
newFooLight = analogRead(ldrPins[0]); // Measure again
if (debug){
Serial.print("Search For:");
Serial.print(fooLight);
Serial.print(" [");
Serial.print(fooLight - lightThreshold);
Serial.print("] Reading:");
Serial.println(newFooLight);
}
delay(20); // Wait for measurment
}
while (newFooLight <= (fooLight - lightThreshold) && newFooLight < maxLightThreshold && noObstacle() && buttonState);
if (debug) Serial.println("Turning is Over.");
}
/*
* Function to check Max Light Direction
*/
int checkLight()
{
int lightDir = 0; // Var to return the higher light source direction
int value = 0; // Store each LDR readings here, init by the higher value 0 = Total Dark
for(int n = 0 ; n < 4 ; n++) // Go through all the LDRs one by one
{
int ldrValue = 0; // <
for (int foo = 0 ; foo < measurment ; foo++)// |
{ // | Take measurments from each for better accuracy
ldrValue += analogRead(ldrPins[n]); // |
} // |
ldrValue /= measurment; // <
if (debug){
Serial.print("No:");
Serial.print(n);
Serial.print(" LDR Value :");
Serial.println(ldrValue);
}
if( ldrValue > value ) // If the new reading is lower (Light strength higer)
{
value = ldrValue; // Keep the new value
lightDir = n; // Keep its direction
}
}
return lightDir; // Return the stronger light Direction
}
/*
* Scan for an obstacle
*/
int noObstacle() // Function return: HIGH/1 if not, LOW/0 if yes
{
if (analogRead(irSensorPin) > minObstacleDistance) // If we found an obstacle
{
return 0; // make it FALSE
}
else
{
return 1; // if we dont make it TRUE
}
}
void move(int moveDir) // Move Function
{
switch (moveDir) {
case 0: // Front
if (debug > 1) Serial.println("We are going ahead.");
moveFront(500);
break;
case 1: // Right
if (debug > 1) Serial.println("We are going right.");
moveRight(500);
break;
case 2: // Avoid Obstacle ????
if (debug > 1) Serial.println("We are going back.");
// TO DO !!!!! Here we have to make rover turn around 180 degrees !!!!!!!!
// moveRandomBack();
moveBack(500);
break;
case 3: // Left
if (debug > 1) Serial.println("We are going left.");
moveLeft(500);
break;
}
}
/*
Functions to make servos move.
moveSpeed as:
0 = stop
to
500 = full speed
*/
void moveFront(int moveSpeed)
{
moveSpeed = map(moveSpeed, 0, 500, servoCenter, maxServoPulse);
int reverseSpeed = servoCenter-(moveSpeed-servoCenter); // Reverse speed for oposite servo
if (millis() - lastPulse >= refreshTime)
{
digitalWrite(servoPinLeft, HIGH); // start the pulse
delayMicroseconds(reverseSpeed); // pulse width
digitalWrite(servoPinLeft, LOW); // stop the pulse
digitalWrite(servoPinRight, HIGH); // start the pulse
delayMicroseconds(moveSpeed); // pulse width
digitalWrite(servoPinRight, LOW); // stop the pulse
lastPulse = millis(); // save the time of the last pulse
}
}
void moveBack(int moveSpeed)
{
moveSpeed = map(moveSpeed, 0, 500, servoCenter, maxServoPulse);
int reverseSpeed = servoCenter-(moveSpeed-servoCenter); // Reverse speed for oposite servo
if (millis() - lastPulse >= refreshTime)
{
digitalWrite(servoPinLeft, HIGH); // start the pulse
delayMicroseconds(moveSpeed); // pulse width
digitalWrite(servoPinLeft, LOW); // stop the pulse
digitalWrite(servoPinRight, HIGH); // start the pulse
delayMicroseconds(reverseSpeed); // pulse width
digitalWrite(servoPinRight, LOW); // stop the pulse
lastPulse = millis(); // save the time of the last pulse
}
}
void moveLeft(int moveSpeed)
{
moveSpeed = map(moveSpeed, 0, 500, servoCenter, maxServoPulse);
int reverseSpeed = servoCenter-(moveSpeed-servoCenter); // Reverse speed for oposite servo
if (millis() - lastPulse >= refreshTime)
{
digitalWrite(servoPinRight, HIGH); // start the pulse
delayMicroseconds(moveSpeed); // pulse width
digitalWrite(servoPinRight, LOW); // stop the pulse
digitalWrite(servoPinLeft, HIGH); // start the pulse
delayMicroseconds(moveSpeed); // pulse width
digitalWrite(servoPinLeft, LOW); // stop the pulse
lastPulse = millis(); // save the time of the last pulse
}
}
void moveRight(int moveSpeed)
{
moveSpeed = map(moveSpeed, 0, 500, servoCenter, maxServoPulse);
int reverseSpeed = servoCenter-(moveSpeed-servoCenter); // Reverse speed for oposite servo
// int reverseSpeed = map(moveSpeed, 0, 500, minServoPulse, servoCenter);
if (millis() - lastPulse >= refreshTime)
{
digitalWrite(servoPinLeft, HIGH); // start the pulse
delayMicroseconds(reverseSpeed); // pulse width
digitalWrite(servoPinLeft, LOW); // stop the pulse
digitalWrite(servoPinRight, HIGH); // start the pulse
delayMicroseconds(reverseSpeed); // pulse width
digitalWrite(servoPinRight, LOW); // stop the pulse
lastPulse = millis(); // save the time of the last pulse
}
}
void moveRandomBack(int timeToMoveBack)
{
long timeStart = millis();
int tempDir = random(1,4);
if (tempDir == 2) tempDir = 1;
if (debug) Serial.println("Move Back.");
while(millis() < (timeStart + timeToMoveBack))
{
moveBack(500);
}
timeStart = millis();
if (debug) Serial.print("MoveRandom:");
if (debug) Serial.println(tempDir);
while(millis() < (timeStart + timeToMoveBack) && noObstacle())
{
move(tempDir);
}
}
void doDance (int timeToDance)
{
digitalWrite(ledPin, HIGH);
long timeStart;
if (debug) Serial.println("..... Let's DANCE !!!!!....");
delay(1000); // Wait for 1sec
timeToDance /=4 ;
for (int foo=1 ; foo < 5; foo++)
{
timeStart = millis();
while(millis() < (timeStart + 500))
{
move(2);
}
timeStart = millis();
while(millis() < (timeStart + 500))
{
move(0);
}
}
timeStart = millis();
while(millis() < (timeStart + timeToDance))
{
move(1);
}
timeStart = millis();
while(millis() < (timeStart + timeToDance))
{
move(3);
}
timeStart = millis();
while(millis() < (timeStart + timeToDance))
{
move(2);
}
timeStart = millis();
while(millis() < (timeStart + timeToDance))
{
move(0);
}
delay(1000); // Wait for 1sec
digitalWrite(ledPin, LOW);
}To see my way to the construction of the robot above you can visit this pagehttp://www.grobot.gr/phpBB3/viewtopic.php?f=2&t=324&st=0&sk=t&sd=a, its in greek but you can see the photos I didn't put them here cause are many .