Raupenfahrzeug auf einem Tisch

Projekt: Bei unserem Projekt soll ein Fahrzeug auf einem Tisch fahren, ohne von diesem runterzufallen.

Bild 1

Code:

//Ultraschall VORNE
const int MessungVorneTrig = 25;
const int MessungVorneEcho = 24;

//Ultraschall HINTEN
const int MessungHintenTrig = 37;
const int MessungHintenEcho = 36;

//Ultraschall RECHTS
const int MessungRechtsTrig = 33;
const int MessungRechtsEcho = 32;

//Ultraschall LINKS
const int MessungLinksTrig = 29;
const int MessungLinksEcho = 28;

//Motor
#include
AF_DCMotor motorRechts(1);
AF_DCMotor motorLinks(2);
const int geschw = 150;      //0 = langsam,  255 = schnell


int status_ = 1;                           //Status    0 = Halt, 1 = Vorwärts,  2 = Rückwärts, 3 = RechtsDrehen, 4 = LinksDrehen --- 5 = RechteRaupeVor, 6 = RechteRaupeZurück, 7 = LinkeRaupeVor, 8 = LinkeRaupeZurück
int status_alt = 0;                        //Status    - " -

//Abstand
float cmVorne = 0;
float cmHinten = 0;
float cmRechts = 0;
float cmLinks = 0;

float abstand1 = 20;     // Grenze


void setup () {
    
  //Ultraschall VORNE
  pinMode(MessungVorneTrig,OUTPUT);
  pinMode(MessungVorneEcho,INPUT);
 
  //Ultraschall HINTEN
  pinMode(MessungHintenTrig,OUTPUT);
  pinMode(MessungHintenEcho,INPUT);
 
  //Ultraschall RECHTS
  pinMode(MessungRechtsTrig,OUTPUT);
  pinMode(MessungRechtsEcho,INPUT);
 
  //Ultraschall LINKS
  pinMode(MessungLinksTrig,OUTPUT);
  pinMode(MessungLinksEcho,INPUT);
 
  //Serieller Monitor
  Serial.begin(9600);
 
  //Motor
  motorRechts.setSpeed(geschw);
  motorRechts.run(RELEASE);
  motorLinks.setSpeed(geschw);
  motorLinks.run(RELEASE);
}


void linksVor()      {  motorLinks.run(FORWARD);}          //Rechte Seite wird dadurch nicht beeinflusst
void linksZurueck()  {  motorLinks.run(BACKWARD);}            //Rechte Seite wird dadurch nicht beeinflusst
void rechtsVor()     {  motorRechts.run(FORWARD);}          //Linke Seite wird dadurch nicht beeinflusst
void rechtsZurueck() {  motorRechts.run(BACKWARD);}            //Linke Seite wird dadurch nicht beeinflusst

void vorwaerts ()    {  linksVor(); rechtsVor();              }
void rueckwaerts ()  {  linksZurueck(); rechtsZurueck();      }
void rechtsdrehen () {  linksVor(); rechtsZurueck();          }
void linksdrehen ()  {  linksZurueck(); rechtsVor();          }
void halt()          {  motorRechts.run(RELEASE); motorLinks.run(RELEASE);    }



void AbstandMessen() {

  //Vorne - Messen
  digitalWrite(MessungVorneTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(MessungVorneTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(MessungVorneTrig, LOW);
  cmVorne = pulseIn(MessungVorneEcho, HIGH) / 58.0;
  cmVorne = (int(cmVorne * 100.0)) / 100.0;            
   Serial.println(cmVorne);
   
  //Hinten - Messen
  digitalWrite(MessungHintenTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(MessungHintenTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(MessungHintenTrig, LOW);
  cmHinten = pulseIn(MessungHintenEcho, HIGH) / 58.0;
  cmHinten = (int(cmHinten * 100.0)) / 100.0;
  Serial.println(cmHinten);
 
  //Rechts - Messen
  digitalWrite(MessungRechtsTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(MessungRechtsTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(MessungRechtsTrig, LOW);
  cmRechts = pulseIn(MessungRechtsEcho, HIGH) / 58.0;
  cmRechts = (int(cmRechts * 100.0)) / 100.0;
  Serial.println(cmRechts);
 
  //Links - Messen
  digitalWrite(MessungLinksTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(MessungLinksTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(MessungLinksTrig, LOW);
  cmLinks = pulseIn(MessungLinksEcho, HIGH) / 58.0;
  cmLinks = (int(cmLinks * 100.0)) / 100.0;
  Serial.println(cmLinks);
}


void RichtungSchalten(){

  //Status    0 = Halt, 1 = Vorwärts,  2 = Rückwärts, 3 = RechtsDrehen, 4 = LinksDrehen --- 5 = RechteRaupeVor, 6 = RechteRaupeZurück, 7 = LinkeRaupeVor, 8 = LinkeRaupeZurück

  if ((cmHinten < abstand1) && (cmVorne < abstand1) && (cmRechts < abstand1) && (cmLinks < abstand1)) {status_ = 1;}  //Ganz Auf dem Tisch
  if (cmVorne > abstand1) { status_ = 6;}      //Vorne........RechtsZurueck  --- Vorne über dem Rand vom Tisch
  if (cmRechts > abstand1) { status_ = 8;}     //Rechts.......LinksZurueck
  if (cmLinks > abstand1) { status_ = 6;}      //Links........RechtsZurueck
  if (cmHinten > abstand1) { status_ = 1;}     //Hinten.......Vor
 
  if ((cmVorne > abstand1) && (cmLinks > abstand1)) { status_ = 6;}       //Vorne && Links    
  if ((cmVorne > abstand1) && (cmRechts > abstand1)) { status_ = 8;}      //Vorne && Rechts
  if ((cmHinten > abstand1) && (cmLinks > abstand1)) { status_ = 7;}      //Hinten && Links
  if ((cmHinten > abstand1) && (cmRechts > abstand1)) { status_ = 5;}     //Hinten && Rechts
 
//2 Sensoren....Vorne-Hinten/Rechts-Links
  if ((cmRechts > abstand1) && (cmLinks > abstand1)) { status_ = 1;}      //Rechts && Links
  if ((cmVorne > abstand1) && (cmHinten > abstand1)) { status_ = 3;}      //Vorne && Hinten
 
//3 Sensoren
  if ((cmVorne > abstand1) && (cmRechts > abstand1) && (cmLinks > abstand1)) {status_ = 4;}
  if ((cmVorne > abstand1) && (cmHinten > abstand1) && (cmRechts > abstand1)) {status_ = 4;}
  if ((cmVorne > abstand1) && (cmHinten > abstand1) && (cmLinks > abstand1)) {status_ = 3;}
  if ((cmHinten > abstand1) && (cmRechts > abstand1) && (cmLinks > abstand1)) {status_ = 1;}
 
//Alle sensoren zeigen zu hohen wert an - z.B. durch hochheben
  if ((cmHinten > abstand1) && (cmVorne > abstand1) && (cmRechts > abstand1) && (cmLinks > abstand1)) {status_ = 0;}

  if (status_alt != status_) {
    halt();
    //delay(50);
    if (status_ == 1)  { vorwaerts();}
    if (status_ == 2)  { rueckwaerts();}
    if (status_ == 3)  { rechtsdrehen();}
    if (status_ == 4)  { linksdrehen();}
    if (status_ == 5)  { rechtsVor();}
    if (status_ == 6)  { rechtsZurueck();}
    if (status_ == 7)  { linksVor();}
    if (status_ == 8)  { linksZurueck();}
    if (status_ == 0)  { halt();}
    status_alt = status_;
    Serial.println(status_);
  }
}


void loop () {
  AbstandMessen();
  RichtungSchalten();
}

 

 

Ein Projekt von Mark Witt, Julius Meier, Lea Hofmann, Jonas Gödecke und Paul van den Berg.