Hey there, I have a problem with my sketch:
#include <ArduinoRobot.h>
#include <Wire.h>
#include <SPI.h>
const int serialPeriod = 500; // Gibt alle viertel Sekunden die Werte auf der Konsole wieder
unsigned long timeSerialDelay = 0;
const int loopPeriod = 20;
unsigned long timeLoopDelay = 0;
// Benennt den Echo & Trig Pin die für den Ultraschallsensor gebraucht werden
const int ultrasonic2TrigPin = TKD3;
const int ultrasonic2EchoPin = TKD4;
int ultrasonic2Distance;
int ultrasonic2Duration;
// definiere Werte
#define DRIVE_FORWARD 0
#define TURN_LEFT 1
int state = DRIVE_FORWARD;
void setup()
{
Serial.begin(9600); //Übertragungsgeschwindigkeit für die Kommunikation mit dem Computer
Robot.begin();
Robot.beginTFT();
// Ultraschallsensor Pin Konfiguration
pinMode (ultrasonic2TrigPin, OUTPUT);
pinMode (ultrasonic2EchoPin, INPUT);
}
void loop()
{
debugOutput(); // Gibt die Werte des Ultraschalssensor's in der Konsole an
if(millis() - timeLoopDelay >= loopPeriod)
{
readUltrasonicSensors(); //ließt und speichert die gemessene Distanz
stateMachine();
timeLoopDelay = millis();
}
}
void stateMachine()
{
if(state == DRIVE_FORWARD) // keine Hindernisse vor mir
{
if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // Wenn der Weg frei ist
{
//Fahre vorwärts
Robot.motorsWrite(128, 128);
}
else // Hindernis vor mir
{
state = TURN_LEFT;
}
}
else if(state == TURN_LEFT) // Hindernis im Weg, dreh nach links
{
unsigned long timeToTurnLeft = 1100; // Es braucht -+ 1.1 Sekunden um 90° zu drehen
unsigned long turnStartTime = millis(); //Speichert die Zeit als der Robot mit dem Drehen gestartet ist
while((millis()-turnStartTime < timeToTurnLeft)) // bleibt in der Schleife bis timeToTurnLeft (1.1 Sekunden) vorbei ist
{
// dreh nach links
Robot.turn(90);
}
state = DRIVE_FORWARD;
}
}
void readUltrasonicSensors()
{
// Ultraschalssensor 2
Robot.digitalWrite(ultrasonic2TrigPin, HIGH);
delayMicroseconds(10); // der TrigPin muss für mindestens 10 us an bleiben
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();
}
}
the compiler say there are no errors but when I upload the sketch to the ArduinoRobot, he doesn't move, he just give the distance between him and an object on the tft screen out.
P.S: It,s the original Robot from Arduino, http://arduino.cc/en/Main/Robot