Jump to content


Photo

modifying funduino robot code for project.


  • Please log in to reply
25 replies to this topic

#1 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 19 February 2015 - 10:30 PM

I'm trying to modify the funduino robot code so that the robot I have built follows LED's on a light grid I've constructed, I'm new to arduino and am struggling how to implement this in the code. I have modified the robot so that it has a chassis mounted LDR which is fed into an analogue pin on the UNO board, I'm not entirely sure how to implement this in the code though?

 

If possible I want to add an argument which dictates that when the LDR receives a set threshold from the the LED light source it will activate set movements before resetting to receive the next light value, I also need to keep the distance sensor as the LED light grid is built on a table with walls that stop the robot from riding over the edge.

 

If anyone can help me with this I'd be extremely grateful :)


  • 0

#2 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 24 February 2015 - 12:01 AM

Hi,
 
I'm really sorry for not getting back with you sooner. 
 
This sounds like a really cool project! 
 

I'm trying to modify the funduino robot code so that the robot I have built follows LED's on a light grid I've constructed, I'm new to arduino and am struggling how to implement this in the code. I have modified the robot so that it has a chassis mounted LDR which is fed into an analogue pin on the UNO board, I'm not entirely sure how to implement this in the code though?

 

Can you explain what you mean by a "light grid"? Maybe post a pic if you have one?

You're talking about a photoresistor like the one below, right?

 

photoresistor-784-218.jpg

How did you wire it to the Arduino? Connecting the photoresistor directly from 5V or GND to the analog input pin will not allow you to measure the change in light intensity. What you'll have to do is use the LDR to make a voltage divider whose output will vary with the resistance of the LDR. Like this:

 

arduino_photocell1.png

 

As explained in this tutorial.

 

That way, when the intensity of light changes, the output of the voltage divider (going to the analog input) will change, which you can monitor in your program.

 

After wiring your circuit like that, you can test the photoresistor with code like this:

int analogPin = 1;
int photoresistorValue = 0;

void setup()
{
  Serial.begin(9600);


void loop()
{
  photoresistorValue = analogRead(analogPin);

  Serial.println(photoresistorValue);
}

Verify it works correctly (you should see the value changing with the intensity of the light), and then you can determine what threshold values to use in your state machine.

 

If possible I want to add an argument which dictates that when the LDR receives a set threshold from the the LED light source it will activate set movements before resetting to receive the next light value, I also need to keep the distance sensor as the LED light grid is built on a table with walls that stop the robot from riding over the edge.
 
If anyone can help me with this I'd be extremely grateful :)

 

Are you still wanting to use the ultrasonic sensor to avoid obstacles?

 

What kind of pre-programmed movements are you wanting the robot to execute when it drives over an LED?


  • 0

#3 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 02 March 2015 - 06:30 PM

Hi Nathan,

 

Thanks for getting back to me, with the LDR i've simply soldered it to some twin core cable so I can run it to the breadboard, I've soldered stip pins on the other end so I can mount it. I spoke to some guys on the arduino forum as well and they said about using a voltage divider circuit, looking at the diagram you've posted I kind of understand the concept but applying it practically is where I'm struggling.

 

With the robot I still want to use the distance sensor as the table has sides which i've built specifically for this, i'm unsure as of yet how I want the robot to move when the LDR executes the code but I want it to follow a kind of langton's ant principle.The problem is it's a university project and I have about four weeks to finish it, I have a blog online which details the build process (it's got a lot of thereoretical background as well which you may want to ignore) but shows what i'm trying to achieve, it's for a sound art installation that looks at the principles of pseudo random composition and music created by chance. 


  • 0

#4 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 04 March 2015 - 02:44 PM

Were you able to build the circuit and try out the code I posted? If not, try building it and see what happens. Once you are able to read from the photoresistor, then I can help you modify the state machine in the code to implement the additional sensor.

 

If you have any trouble building the circuit I posted, just let me know and I'll try to help out!


  • 0

#5 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 04 March 2015 - 07:49 PM

I built the circuit and have a print out in the serial monitor, the value is reading around 216 when exposed to the LED, this is with a 10k resistor,It drops to around 206 when the LDR is covered. The LEDs on the board are much brighter than the ones I'm using in this circuit, I should be able to modify the values when I have it in the code though shouldn't I if it's done by ranges?

 

Oh I ran voltage to the LED as well of the resistor leg that runs to the digital pin to light it up, is that right or will that disturb the values?


Edited by Si_Price, 04 March 2015 - 07:56 PM.

  • 0

#6 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 04 March 2015 - 08:34 PM

Something's not right. The difference should be much greater than that. Ideally the value should be around 0 or 1024 depending on whether the photoresistor is exposed to light (realistically it won't be perfect like that, but there should be at least a difference of a few hundred).

Can you post a pic of your wiring? We need to get this working before starting on the modifying the state machine..


  • 0

#7 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 04 March 2015 - 08:58 PM

I've just tested the LDR using the AnalogReadSerial sketch and it's showing larger values, I removed the resistor from the LED to make it brighter which seems to be the issue I'm now getting much larger readings. It now jumps from 34 in a dark room to 671 when the LED is turned on. 


  • 0

#8 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 04 March 2015 - 11:12 PM

Ok, that's good. So you know your circuit is working now. Once you start using your robot on your light grid you can adjust the threshold value used to determine whether it's over a light or not.

 

If you look at the code provided in the funduino robotics kit guide, you'll notice a finite state machine is used to control the behavior of the robot. If you want to make the behavior change when the robot drives over an LED, you'll need to modify the state machine.

 

Let's take a look at what the state machine looks like now:

void stateMachine()
{
    if(state == DRIVE_FORWARD) // no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            // drive forward
        }
        else // there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }
    else if(state == TURN_LEFT) // obstacle detected -- turn left
    {
        // ... do stuff
        
        state = DRIVE_FORWARD;
    }
}

What you need to do is add an additional state for the case where the an LED is detected. First, add your photoresistor code that you are using to the program somewhere, making sure that the photoresistorValue variable (or whatever you name it) is updated often, like the ultrasonic sensor is. Then, you can add an additional state like this:
 

void stateMachine()
{
    if(state == DRIVE_FORWARD) // no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            // drive forward
        }
        else if(photoresistorValue > 500 ) // if an LED is detected
        {
                     state = YOUR_STATE; // change the state to your state, which can make the robot do whatever you want..
               }
        else // there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }
    else if(state == TURN_LEFT) // obstacle detected -- turn left
    {
        // ... do stuff
        
        state = DRIVE_FORWARD;
    }
    else if(state == YOUR_STATE)
    {
        // ... do stuff
        
        state = DRIVE_FORWARD; // after you do your thing, you can revert back to the default state
    }
}

Make sense?


  • 0

#9 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 05 March 2015 - 10:18 AM

I think so, I've started writing the code but I'm getting some verification errors?

#include <Servo.h>

// create servo objects
Servo leftMotor;
Servo rightMotor;

const int serialPeriod = 250; // serial print to console every 1/4 second
unsigned long timeSerialDelay =0;

const int loopPeriod = 20;   // ultrasonic period and frequency (50hz)
unsigned long timeLoopDelay =0;

// pinrs used for ultrasonic sensor
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

int ultrasonic2Distance;
int ultrasonic2Duration;

// LDR analog pin

const int sensorValue = analogRead(A0);   //LDR on Analog PIN 0
  // print out the value you read:
  Serial.println(sensorValue);  // Print values to console
  delay(1); // Delay between reads



#define DRIVE_FORWARD 0   //defined states
#define TURN_LEFT     1
#define MY_STATE      2


int state = DRIVE_FORWARD;

void setup()

{
    Serial.begin(9600);
    
        // ultrasonic sensor pin configurations
    pinMode(ultrasonic2TrigPin, OUTPUT);
    pinMode(ultrasonic2EchoPin, INPUT);
    
    leftMotor.attach(13);
    rightMotor.attach(12);
}
    
void loop()


 

    debugOutput(); // prints debugging messages to the serial console
    
    if(millis() - timeLoopDelay >= loopPeriod)
    {
        readUltrasonicSensors(); // read and store the measured distances
        
        stateMachine();
        
        timeLoopDelay = millis();
    }
}


void stateMachine()
{
    if(state == DRIVE_FORWARD) // no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            // drive forward
              rightMotor.write(180);
            leftMotor.write(0);
        }
        else if(sensorValue > 500 ) // if an LED is detected
        {
                     state = MY_STATE; // change the state to your state, which can make the robot do whatever you want..
        rightMotor.write (90);
        leftMotor.write  (90);
        
        RightMotor.write (-180);
        leftMotor.write  (-180);
        
        rightMotor.write (180);
        leftMtor.write   (0);
        
       }
        else // there's an object in front of us
        {
            state = TURN_LEFT;
            
               }
    }
    else if(state == TURN_LEFT) // obstacle detected -- turn left
    {
        unsigned long timeToTurnLeft = 500; // it takes around .5 seconds to turn 90 degrees
        
        unsigned long turnStartTime = millis(); // save the time that we started turning

        while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until timeToTurnLeft (.5 seconds) has elapsed
        {
            // turn left
            rightMotor.write(180);
            leftMotor.write(180);
        }
        
        state = DRIVE_FORWARD; // after you do your thing, you can revert back to the default state
    }
}


void readUltrasonicSensors()
{
    // ultrasonic 2
    digitalWrite(ultrasonic2TrigPin, HIGH);
    delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
    digitalWrite(ultrasonic2TrigPin, LOW);
    
    ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);
    ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}
//

void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
        Serial.print("ultrasonic2Distance: ");
        Serial.print(ultrasonic2Distance);
        Serial.print("cm");
        Serial.println();
        
        timeSerialDelay = millis();
    }
}

With Serial.Print in i'm getting these messages:

 

expected constructor, destructor, or type conversion before '.' token
error: expected declaration before '}' token

 

 

I'm guessing I'm going to get a ton of compiler errors but this is the first time I've tried to do something like this!


  • 0

#10 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 06 March 2015 - 12:26 AM

One problem is that you're using Serial before it's been configured. You have "Serial.println(sensorValue);" outside of any function, which isn't even allowed (all code must go inside a function). You need to move that line, along with the analogRead() line above it and delay line below it inside a function.

 

You also had lines writing to the motors with negative values, which isn't allowed. the ".write()" function only accepts values between 0 (full speed one direction) and 180 (full speed other direction, with 90 being neutral).

 

Here is modified code that compiles, but I haven't actually tested:

#include <Servo.h>

// create servo objects
Servo leftMotor;
Servo rightMotor;

const int serialPeriod = 250; // serial print to console every 1/4 second
unsigned long timeSerialDelay =0;

const int loopPeriod = 20;   // ultrasonic period and frequency (50hz)
unsigned long timeLoopDelay =0;

// pins used for ultrasonic sensor & LDR
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

const int ldrPin = A0;

int ultrasonic2Distance;
int ultrasonic2Duration;
int ldrValue;

#define DRIVE_FORWARD 0   // defined states
#define TURN_LEFT     1
#define MY_STATE      2

int state = DRIVE_FORWARD;

void setup()
{
    Serial.begin(9600);
    
    // ultrasonic sensor pin configurations
    pinMode(ultrasonic2TrigPin, OUTPUT);
    pinMode(ultrasonic2EchoPin, INPUT);
    
    leftMotor.attach(13);
    rightMotor.attach(12);
}
    
void loop()
{
    debugOutput(); // prints debugging messages to the serial console
    
    if(millis() - timeLoopDelay >= loopPeriod)
    {
        readUltrasonicSensors(); // read and store the measured distances
        readLdr(); // read and store the LDR value
        
        stateMachine();
        
        timeLoopDelay = millis();
    }
}


void stateMachine()
{
    if(state == DRIVE_FORWARD) // no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            // drive forward
            rightMotor.write(180);
            leftMotor.write(0);
        }
        else if(ldrValue > 500 ) // if an LED is detected
        {
            state = MY_STATE; // change the state to your state, which can make the robot do whatever you want..
            rightMotor.write (90);
            leftMotor.write  (90);
        }
        else // there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }
    else if(state == TURN_LEFT) // obstacle detected -- turn left
    {
        unsigned long timeToTurnLeft = 500; // it takes around .5 seconds to turn 90 degrees
        
        unsigned long turnStartTime = millis(); // save the time that we started turning

        while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until timeToTurnLeft (.5 seconds) has elapsed
        {
            // turn left
            rightMotor.write(180);
            leftMotor.write(180);
        }
        
        state = DRIVE_FORWARD; // after you do your thing, you can revert back to the default state
    }
}


void readUltrasonicSensors()
{
    // ultrasonic 2
    digitalWrite(ultrasonic2TrigPin, HIGH);
    delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
    digitalWrite(ultrasonic2TrigPin, LOW);
    
    ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);
    ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}


void readLdr()
{
    ldrValue = analogRead(ldrPin);
}

void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
        Serial.print("ultrasonic2Distance: ");
        Serial.print(ultrasonic2Distance);
        Serial.print("cm");
        Serial.println();
        
        Serial.print("ldrValue: ");
        Serial.print(ldrValue);
        Serial.println();
        
        timeSerialDelay = millis();
    }
}

  • 0

#11 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 06 March 2015 - 09:11 PM

just verified it works perfect, thanks so much for your help I really appreciate it. Once I've got the project up and running I'll post a link to the performance so people can see the results!


  • 1

#12 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 08 March 2015 - 04:35 PM

Hi Nathan,

 

Just tried running the above code on my robot and I've got some issues, the robot now drives backwards and the distance sensor doesn't seem to be working in the code, I checked the serial monitor and I'm getting both readouts for the LDR and ultra sonic distance senor but this has not effect on the robot when the code is running? Also occasionally the robot spins for 360 degress then continues to reverse?

 

 

EDIT: it was running backwards because I wired up the pins wrong, the Distance sensor does seem to be working but intermittently? I'll put my hand in front of it and sometimes it will read sometimes it won't?


  • 0

#13 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 10 March 2015 - 02:03 PM

Ok, so now the only problem is that the distance sensor seems to be working intermittently? 

 

You'll need to do some debugging. Try downloading the ultrasonic test code from the robotics guide programming page and test the sensor without all of the other code. After downloading it, put something like a piece of paper or book in front of the sensor (your hand may not be large enough to block the sensor more than a few inches away) and move it further and closer from the sensor.

 

If the values printing out seem consistent and correct, then switch back to your main program and we can start debugging that. If the values are intermittent or seem incorrect, then we could have a wiring issue or some other problem..


  • 0

#14 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 10 March 2015 - 02:21 PM

I've run tests on the distance sensor and its working fine I've also reverted back to the original code from the funduino project  and the robot works as expected it's just not working in the above code, I've tested the Photoresistor also and its reading and printing the values fine but when the threshold passes 500 the state doesn't change either.


  • 0

#15 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 10 March 2015 - 03:41 PM

I see the problem (or one problem, at least). The drive forward state is:

if(state == DRIVE_FORWARD) // no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            // drive forward
            rightMotor.write(180);
            leftMotor.write(0);
        }
        else if(ldrValue > 500 ) // if an LED is detected
        {
            state = MY_STATE; // change the state to your state, which can make the robot do whatever you want..
            rightMotor.write (90);
            leftMotor.write  (90);
        }
        else // there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }

If there's nothing in front of the robot, then the program will branch into the first IF statement, meaning it will never go into the ELSE IF after it. You'll need to modify this logic to accomplish the behavior you want (i.e. which sensor do you want to give precedence? Or, you could find a way to give them equal precedence..).

 

Fixing that should make the LDR state work.

 

As for the ultrasonic sensor working intermittently with the above program, you'll have to do some debugging to figure out what the problem is. My suggestion would be to put debug (print line) statements in each of the states, as well as other key areas in the program, and then run it and observe the output. Looking over the debug output will show you how the program executed, which will allow you to determine if the program executed correctly or if there's a bug.


  • 0

#16 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 10 March 2015 - 04:44 PM

Sorry how would I go about debugging each state would I need to copy the debugOutput into each state and then see what the serial monitor outputs at each stage in the code?

void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
        Serial.print("ultrasonic2Distance: ");
        Serial.print(ultrasonic2Distance);
        Serial.print("cm");
        Serial.println();
        
        Serial.print("ldrValue: ");
        Serial.print(ldrValue);
        Serial.println();
        
        timeSerialDelay = millis();
    }

  • 0

#17 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 10 March 2015 - 04:48 PM

No, just add Serial.println() statements in key parts of the program. The idea is that after running the program, you will be able to look at the output and see how it executed.

 

Example:

if(state == DRIVE_FORWARD) // no obstacles detected
    {
        Serial.println("inside DRIVE_FORWARD"); // debug
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
        {
            Serial.println("inside DRIVE_FORWARD, ultrasonic branch"); // debug
            // drive forward
            rightMotor.write(180);
            leftMotor.write(0);
        }
        else if(ldrValue > 500 ) // if an LED is detected
        {
            Serial.println("inside DRIVE_FORWARD, ldr branch"); // debug
            state = MY_STATE; // change the state to your state, which can make the robot do whatever you want..
            rightMotor.write (90);
            leftMotor.write  (90);
        }
        else // there's an object in front of us
        {
            Serial.println("inside DRIVE_FORWARD, object in front of us branch"); // debug
            state = TURN_LEFT;
        }
    }

That way you can inspect the output and see if the program is functioning as intended.

 

As I mentioned before, you will have to change the logic of the state machine as it's currently incorrect.


  • 0

#18 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 10 March 2015 - 04:54 PM

right okay, man coding is hard, how do I approach changing the state machine? I look at the code I understand what it's doing I just dont get how the language works it's like using hand gestures to communicate with someone in a foreign language.


  • 0

#19 Nathan House

Nathan House

    Administrator

  • Administrators
  • 122 posts
  • LocationLynchburg, VA

Posted 10 March 2015 - 09:50 PM

right okay, man coding is hard, how do I approach changing the state machine? I look at the code I understand what it's doing I just dont get how the language works it's like using hand gestures to communicate with someone in a foreign language.

 

Ha ha, programming can be a little daunting at first, but with a little (or maybe a lot :-P) of effort you can get the hang of it.

 

I would check out the tutorials on the main Arduino website here: http://arduino.cc/en/Tutorial/HomePage

 

In particular, read the tutorials under "5.Control Structures."

 

I won't rewrite the whole program for you, but here's the drive forward state rewritten in such a way that I think accomplishes what you want:

if(state == DRIVE_FORWARD) {
    Serial.println("inside DRIVE_FORWARD"); // debug
    
    if(ultrasonic2Distance > 0 && ultrasonic2Distance < 6) { // there's an object in front of us
        Serial.println("ultrasonic2Distance > 0 && ultrasonic2Distance < 6"); // debug
        state = TURN_LEFT;
    } else if(ldrValue > 500) { // if an LED is detected
        Serial.println("ldrValue > 500"); // debug
        state = MY_STATE; // next loop the program will branch into MY_STATE
    } else { // there's an object in front of us
        Serial.println("nothing in front of us and no LED detected, driving forward.."); // debug
        // drive forward
        rightMotor.write(180);
        leftMotor.write(0);
    }
}

Arduino is programmed using a C-like language, so if you really want to learn how to program Arduino's well you should try going through some C tutorials. This is a really good site: http://www.cprogramm...c-tutorial.html


  • 0

#20 Si_Price

Si_Price

    Member

  • Members
  • PipPip
  • 15 posts

Posted 11 March 2015 - 11:33 AM

I'll have to have a look I did buy a kit that had a load of examples and I've started working my way through them, it's things like blinking LED's and stuff but I'm definitely going to get more into it when i finish university.

 

I've added the code in where I think it should be and I'm still not getting any changes to either the Sensor or the LDR, I think I'm going to have to admit defeat on this. I've only got two and a half weeks until It needs to be ready and there's still other aspects of the project I'm yet to finish, Here's the newly written code:

#include <Servo.h>

// create servo objects
Servo leftMotor;
Servo rightMotor;

const int serialPeriod = 250; // serial print to console every 1/4 second
unsigned long timeSerialDelay =0;

const int loopPeriod = 20;   // ultrasonic period and frequency (50hz)
unsigned long timeLoopDelay =0;

// pins used for ultrasonic sensor,LDR and LED 
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

const int ldrPin = A0;
int led = 3;

int ultrasonic2Distance;
int ultrasonic2Duration;
int ldrValue;

#define DRIVE_FORWARD 0   // defined states
#define TURN_LEFT     1
#define MY_STATE      2

int state = DRIVE_FORWARD;

void setup()
{
    Serial.begin(9600);
    
    // ultrasonic sensor pin configurations
    pinMode(ultrasonic2TrigPin, OUTPUT);
    pinMode(ultrasonic2EchoPin, INPUT);
     pinMode(led, OUTPUT); 
     
    leftMotor.attach(13);
    rightMotor.attach(12);
}
    
void loop()
{
    debugOutput(); // prints debugging messages to the serial console
    
    if(millis() - timeLoopDelay >= loopPeriod)
    {
        readUltrasonicSensors(); // read and store the measured distances
        readLdr(); // read and store the LDR value
        
        stateMachine();
        
        timeLoopDelay = millis();
        
          digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
  delay(1000);               // wait for a second
  digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW
  delay(1000);               // wait for a second
    }
}


void stateMachine()
{
    if(state == DRIVE_FORWARD) {
    Serial.println("inside DRIVE_FORWARD"); // debug
    
    if(ultrasonic2Distance > 0 && ultrasonic2Distance < 6) { // there's an object in front of us
        Serial.println("ultrasonic2Distance > 0 && ultrasonic2Distance < 6"); // debug
        state = TURN_LEFT;
    } else if(ldrValue > 500) { // if an LED is detected
        Serial.println("ldrValue > 500"); // debug
        state = MY_STATE; // next loop the program will branch into MY_STATE
    }
  else // there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }
    else if(state == TURN_LEFT) // obstacle detected -- turn left
    {
        unsigned long timeToTurnLeft = 500; // it takes around .5 seconds to turn 90 degrees
        
        unsigned long turnStartTime = millis(); // save the time that we started turning

        while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until timeToTurnLeft (.5 seconds) has elapsed
        {
            // turn left
            rightMotor.write(180);
            leftMotor.write(180);
        }
        Serial.println("nothing in front of us and no LED detected, driving forward.."); // debug
        // drive forward
        rightMotor.write(180);
        leftMotor.write(0);
    }  
        state = DRIVE_FORWARD; // after you do your thing, you can revert back to the default state
    }



void readUltrasonicSensors()
{
    // ultrasonic 2
    digitalWrite(ultrasonic2TrigPin, HIGH);
    delayMicroseconds(10);                  // must keep the trig pin high for at least 10us
    digitalWrite(ultrasonic2TrigPin, LOW);
    
    ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);
    ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}


void readLdr()
{
    ldrValue = analogRead(ldrPin);
}

void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
        Serial.print("ultrasonic2Distance: ");
        Serial.print(ultrasonic2Distance);
        Serial.print("cm");
        Serial.println();
        
        Serial.print("ldrValue: ");
        Serial.print(ldrValue);
        Serial.println();
        
        timeSerialDelay = millis();
    }
}

Do you think it would be easier if i omitted the distance sensor from the code and just used the LDR?, it seems that the two states don't like working with one another so maybe if it was simplified it would work how I perceive it in my head?


  • 0




0 user(s) are reading this topic

0 members, 0 guests, 0 anonymous users