Fahrzeug von SainSmart
https://learn.adafruit.com/adafruit-motor-shield/library-install
// Testprogramm für den Roboter von Sainsmart.com
const int links_richtung = 9; // IN 4, HIGH = Vorwärts, LOW = Rückwärts
const int rechts_richtung = 10; // IN 1, HIGH = Vorwärts, LOW = Rückwärts
const int rechts_geschwindigkeit = 11; // IN 2, 0 = langsam, 255 = schnell
const int links_geschwindigkeit = 12; // IN 3, 0 = langsam, 255 = schnell
int ganzlangsam = 50; // Auf ganz langsam wird im Programm verzichtet, da sich bei schwachen Batterien die Motoren nicht mehr drehen
int langsam = 100;
int mittel = 150;
int schnell = 200;
int ganzschnell = 250;
void setup () {
Serial.begin(9600);
pinMode(links_richtung,OUTPUT);
pinMode(rechts_richtung,OUTPUT);
pinMode(links_geschwindigkeit,OUTPUT);
pinMode(rechts_geschwindigkeit,OUTPUT);
}
void linksvor(int v) { digitalWrite(links_richtung,HIGH); analogWrite(links_geschwindigkeit,255-v);} //Rechte Seite wird dadurch nicht beeinflusst
void linkszurueck(int v) { digitalWrite(links_richtung,LOW); analogWrite(links_geschwindigkeit,v);} //Rechte Seite wird dadurch nicht beeinflusst
void rechtsvor(int v) { digitalWrite(rechts_richtung,HIGH); analogWrite(rechts_geschwindigkeit,255-v);} //Linke Seite wird dadurch nicht beeinflusst
void rechtszurueck(int v) { digitalWrite(rechts_richtung,LOW); analogWrite(rechts_geschwindigkeit,v);} //Linke Seite wird dadurch nicht beeinflusst
void vorwaerts (int v) { linksvor(v); rechtsvor(v); }
void rueckwaerts (int v) { linkszurueck(v); rechtszurueck(v); }
void rechtsdrehen (int v) { linksvor(v); rechtszurueck(v); }
void linksdrehen (int v) { linkszurueck(v); rechtsvor(v); }
void halt() { linkszurueck(0); rechtszurueck(0); }
void loop () {
Serial.println("linksvor(langsam)"); linksvor(langsam); delay(3000); halt(); delay(1000);
Serial.println("linksvor(mittel)"); linksvor(mittel); delay(3000); halt(); delay(1000);
Serial.println("linksvor(schnell)"); linksvor(schnell); delay(3000); halt(); delay(1000);
Serial.println("linkszurueck(langsam)"); linkszurueck(langsam); delay(3000); halt(); delay(1000);
Serial.println("linkszurueck(mittel)"); linkszurueck(mittel); delay(3000); halt(); delay(1000);
Serial.println("linkszurueck(schnell)"); linkszurueck(schnell); delay(3000); halt(); delay(1000);
Serial.println("rechtsvor(langsam)"); rechtsvor(langsam); delay(3000); halt(); delay(1000);
Serial.println("rechtsvor(mittel)"); rechtsvor(mittel); delay(3000); halt(); delay(1000);
Serial.println("rechtsvor(schnell)"); rechtsvor(schnell); delay(3000); halt(); delay(1000);
Serial.println("rechtszurueck(langsam)"); rechtszurueck(langsam); delay(3000); halt(); delay(1000);
Serial.println("rechtszurueck(mittel)"); rechtszurueck(mittel); delay(3000); halt(); delay(1000);
Serial.println("rechtszurueck(schnell)"); rechtszurueck(schnell); delay(3000); halt(); delay(1000);
Serial.println("linksvor(mittel)"); linksvor(mittel); delay(3000); halt(); delay(1000);
Serial.println("linkszurueck(mittel)"); linkszurueck(mittel); delay(3000); halt(); delay(1000);
Serial.println("rechtsvor(mittel)"); rechtsvor(mittel); delay(3000); halt(); delay(1000);
Serial.println("rechtszurueck(mittel)"); rechtszurueck(mittel); delay(3000); halt(); delay(1000);
Serial.println("vorwaerts(mittel)"); vorwaerts(mittel); delay(3000); halt(); delay(1000);
Serial.println("rueckwaerts(mittel)"); rueckwaerts(mittel); delay(3000); halt(); delay(1000);
Serial.println("rechtsdrehen(mittel)"); rechtsdrehen(mittel); delay(3000); halt(); delay(1000);
Serial.println("linksdrehen(mittel)"); linksdrehen(mittel); delay(3000); halt(); delay(1000);
}
// Testprogramm für den Roboter mit 2 Abstandssensoren
const int echovorne = 6;
const int triggervorne = 7;
const int echohinten = 2;
const int triggerhinten = 3;
const int links_richtung = 9; // IN 4, HIGH = Vorwärts, LOW = Rückwärts
const int rechts_richtung = 10; // IN 1, HIGH = Vorwärts, LOW = Rückwärts
const int rechts_geschwindigkeit = 11; // IN 2, 0 = langsam, 255 = schnell
const int links_geschwindigkeit = 12; // IN 3, 0 = langsam, 255 = schnell
int mittel = 150;
int status_ = 0;
int status_alt = 0;
float cmvorne = 0;
float cmhinten = 0;
float abstand = 30;
void setup () {
Serial.begin(9600);
pinMode(links_richtung,OUTPUT);
pinMode(rechts_richtung,OUTPUT);
pinMode(links_geschwindigkeit,OUTPUT);
pinMode(rechts_geschwindigkeit,OUTPUT);
pinMode(triggervorne,OUTPUT);
pinMode(echovorne,INPUT);
pinMode(triggerhinten,OUTPUT);
pinMode(echohinten,INPUT);
}
void linksvor(int v) { digitalWrite(links_richtung,HIGH); analogWrite(links_geschwindigkeit,255-v);} //Rechte Seite wird dadurch nicht beeinflusst
void linkszurueck(int v) { digitalWrite(links_richtung,LOW); analogWrite(links_geschwindigkeit,v);} //Rechte Seite wird dadurch nicht beeinflusst
void rechtsvor(int v) { digitalWrite(rechts_richtung,HIGH); analogWrite(rechts_geschwindigkeit,255-v);} //Linke Seite wird dadurch nicht beeinflusst
void rechtszurueck(int v) { digitalWrite(rechts_richtung,LOW); analogWrite(rechts_geschwindigkeit,v);} //Linke Seite wird dadurch nicht beeinflusst
void vorwaerts (int v) { linksvor(v); rechtsvor(v); }
void rueckwaerts (int v) { linkszurueck(v); rechtszurueck(v); }
void rechtsdrehen (int v) { linksvor(v); rechtszurueck(v); }
void linksdrehen (int v) { linkszurueck(v); rechtsvor(v); }
void halt() { linkszurueck(0); rechtszurueck(0); }
void loop () {
// Vorne Abstand messen
digitalWrite(triggervorne, LOW);
delayMicroseconds(2);
digitalWrite(triggervorne, HIGH);
delayMicroseconds(10);
digitalWrite(triggervorne, LOW);
cmvorne = pulseIn(echovorne, HIGH) / 58.0;
cmvorne = (int(cmvorne * 100.0)) / 100.0;
// Hinten Abstand messen
digitalWrite(triggerhinten, LOW);
delayMicroseconds(2);
digitalWrite(triggerhinten, HIGH);
delayMicroseconds(10);
digitalWrite(triggerhinten, LOW);
cmhinten = pulseIn(echohinten, HIGH) / 58.0;
cmhinten= (int(cmhinten * 100.0)) / 100.0;
Serial.print("Abstand vorne: "); Serial.print(cmvorne); Serial.print(" Abstand hinten: "); Serial.println(cmhinten);
//Status 0 = Halt, 1 = Vorwärts, 2 = Rückwärts
if (((cmvorne > abstand) or (cmhinten > abstand)) and (status_alt == 0)) { status_ = 1;}
if (cmvorne < abstand) { status_ = 2;}
if (cmhinten < abstand) { status_ = 1;}
if ((cmvorne < abstand) and (cmhinten < abstand)) { status_ = 0;}
Serial.print("status_alt: "); Serial.print(status_alt); Serial.print(" status: "); Serial.println(status_);
if (status_alt != status_) {
halt();
delay(50);
if (status_ == 1) { vorwaerts(mittel);}
if (status_ == 2) { rueckwaerts(mittel);}
if (status_ == 0) { halt();}
status_alt = status_;
}
delay(1000); // Damit man am Monitor die Daten verfolgen kann
}
