Banner Ausblenden
Kleines Logo

Willkommen auf PSI-Online

Anne,Marcel,Tim

#include <Wire.h>
                                                               //#include <LiquidCrystal_I2C.h>


                                                              //LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

  const int TrigPin = 2;
  const int EchoPin = 3;
  const int EchoPi = 4;
  float cm2 ;
  float cm;

  int pin = 12;
  short motorL_1 = 5; //Motor L vorwärts     A1_A
  short motorL_2 = 6; //Motor L rückwärts    A1_B
  short motorR_1 = 10; //Motor R vorwärts    B1_A
  short motorR_2 = 11; //Motor R rückwärts   B1_B
  short v = 255;
  int t = 500;
  int l = 0;
  int r = 0;
  long milis;
  long millis2;

void setup() {                                //SETUP-------------------------------------------------------------------------------------------------------------------------
  Serial.begin(9600);
  pinMode(motorL_1, OUTPUT);
  pinMode(motorL_2, OUTPUT);
  pinMode(motorR_1, OUTPUT);
  pinMode(motorR_2, OUTPUT);
 
 
  Serial.begin(9600);
 
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  pinMode(EchoPi, INPUT );
 
                                           //lcd.begin(16, 2);
}                                              //SETUP-------------------------------------------------------------------------------------------------------------------------
void loop() {                               //LOOP-------------------------------------------------------------------------------------------------------------------------
 
 
 
  fahren( v + l, -v + r);

     digitalWrite(TrigPin, LOW);
     delayMicroseconds(2);
     digitalWrite(TrigPin, HIGH);
     delayMicroseconds(10);
     digitalWrite(TrigPin, LOW);
     cm = pulseIn(EchoPin, HIGH) / 58.0;
     cm = (int(cm * 100.0)) / 100.0;
     Serial.print(cm);
     Serial.print("cm");
     Serial.println();

     digitalWrite(TrigPin, LOW);
     delayMicroseconds(2);
     digitalWrite(TrigPin, HIGH);
     delayMicroseconds(10);
     digitalWrite(TrigPin, LOW);
     cm2 = pulseIn(EchoPi, HIGH) / 58.0;
     cm2 = (int(cm2 * 100.0)) / 100.0;
     Serial.print(cm2);
     Serial.print("cm2");
     Serial.println();

                                                            //lcd.setCursor(0,0);
      if(cm2 <=15 || cm2 >=0)
       {
          v = 1;
          l = 254;
          r = 254;
          millis2 = millis();
     fahren( v + l, -v + r);  
     }                                                        //lcd.setCursor(0,1);
                                                             // lcd.print(cm);
 
  if (cm <=30 && cm >= 0)
  {
    v=-255;
    l=0;
    r=0;
    fahren( v + l, -v + r);
    delay(1000);
    v = 1;
    l = 254;
    r = 254;
    milis = millis();
                                                              //lcd.setCursor(0,1);
                                                              //lcd.print("drehen");
  }
  else if(cm >=30 || cm <= 0 && millis()- milis  >= 1000 && millis()- millis2  >= 1000 || cm2 >=15 || cm2 <=0)
  {
  v=-255;
  l=0;
  r=0;

                                                      //lcd.print("gerade");
}
   
 
}                                


void fahren(int links, int rechts) {
  if(links != 0 || rechts != 0){
    Serial.print("Links: ");
    Serial.print(links);
    Serial.print("   Rechts: ");
    Serial.println(rechts);
  }
 
  if(links < 0){
    links = -links;
    analogWrite(motorL_1, 0);
    analogWrite(motorL_2, links);
  }else{
    analogWrite(motorL_1, links);
    analogWrite(motorL_2, 0);    
  }



  if(rechts < 0){
    rechts = -rechts;
    analogWrite(motorR_1, 0);
    analogWrite(motorR_2, rechts);
  }else{
    analogWrite(motorR_1, rechts);
    analogWrite(motorR_2, 0);    
  }
 
}



 

Erstellt: Tim Sandner (19.04.2016) Letzte Änderung: Tim Sandner (17.05.2016)