Jump to content


Arduinorobot

Member Since 23 Feb 2015
Offline Last Active Mar 11 2015 02:58 PM
-----

Topics I've Started

ArduinoRobot

28 February 2015 - 04:10 PM

Hey I have a problem with my sketch I used your sketch for the original ArduinoRobot (http://arduino.cc/en/Main/Robot), because my own sketch had too many delay(); and your sketch was perfectly written, but for me it doesn't work. Do you know maybe where the errors are because the compiler says the sketch has no errors.

Here is the sketch its almost the same as yours I just used another pins and librarie's:

#include <ArduinoRobot.h>
#include <Wire.h>
#include <SPI.h>

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

const int loopPeriod = 20;  // a period of 20ms = a frequency of 50Hz
unsigned long timeLoopDelay = 0;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin = TKD3;
const int ultrasonic2EchoPin = TKD4;

int ultrasonic2Distance;
int ultrasonic2Duration;

// define the states
#define DRIVE_FORWARD    0
#define TURN_LEFT        1

int state = DRIVE_FORWARD;  // 0 = drive forward (DEFAULT), 1 = turn left


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

     // ultrasonic sensor pin configurations
     pinMode (ultrasonic2TrigPin, OUTPUT);
     pinMode (ultrasonic2EchoPin, INPUT);
}

void loop() 
{

     debugOutput(); // prints debugging messages to the serial console

     if(millis() - timeLoopDelay >= loopPeriod)
     {
          readUltrasonicSensors(); // read and store the mesured distance
          
          stateMachine();
          
          timeLoopDelay = millis();
     }
}

void stateMachine()
{
     if(state == DRIVE_FORWARD) // no obstacles detected
     {
          if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there is nothing in front of us (note: ultrasonicDistance will be negative for some ultrasonics if there's nothing in range)
          {
               //Fahre vorwärts            
               Robot.motorsWrite(128, 128);
              
          }
          else  // there is an obstacle in front of us
          {
               state = TURN_LEFT;
          }
     }
     else if(state == TURN_LEFT) // obstacle detected -- turn left
     {
          unsigned long timeToTurnLeft = 1100; // it takes around 1.1 seconds to turn 90 °
          
          unsigned long turnStartTime = millis();  // save the time we started turning
          
          while((millis()-turnStartTime < timeToTurnLeft)) // stay in this loop until timeToTurnLeft (1.1 seconds) has elapsed
        {
               // turn left
              Robot.turn(90);
              delay(1100);
        }
     
          state = DRIVE_FORWARD;
     }
}

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


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