arduino_roboter.ino

//04.01.2016
//Code by Michael Bonacina
//michaelsarduino.blogspot.de

int motor1_1 = 0;
int motor1_2 = 1;
int motor1_speed = 3;
int motor2_1 = 2;
int motor2_2 = 4;
int motor2_speed = 5;
int motor3_1 = 6;
int motor3_2 = 7;
int motor3_speed = 9;
int motor4_1 = 8;
int motor4_2 = 10;
int motor4_speed = 11;

int trig1 = A5;
int trig2 = A4;
int trig3 = A3;

int echo1 = A2;
int echo2 = A1;
int echo3 = A0;

float schallgeschwindigkeit = 331.5+0.6*20;

int led_rot = 12;
int led_gruen = 13;

void setup() {
  pinMode(motor1_1, OUTPUT);
  pinMode(motor1_2, OUTPUT);
  pinMode(motor2_1, OUTPUT);
  pinMode(motor2_2, OUTPUT);
  pinMode(motor3_1, OUTPUT);
  pinMode(motor3_2, OUTPUT);
  pinMode(motor4_1, OUTPUT);
  pinMode(motor4_2, OUTPUT);
  
  pinMode(motor1_speed, OUTPUT);
  pinMode(motor2_speed, OUTPUT);
  pinMode(motor3_speed, OUTPUT);
  pinMode(motor4_speed, OUTPUT);
  
  pinMode(trig1, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(trig3, OUTPUT);
  
  pinMode(echo1, INPUT);
  pinMode(echo2, INPUT);
  pinMode(echo3, INPUT);
  
  pinMode(led_rot, OUTPUT);
  pinMode(led_gruen, OUTPUT);  
  
  
}



void loop() {
 
}



float entfernung1messen()
{
 float entfernung_ges = 0;
 float entfernung_in = 0;
 for(int x=0; x<15; x++)
 { 
 digitalWrite(trig1, LOW);
 delayMicroseconds(4);
 digitalWrite(trig1, HIGH);
 delayMicroseconds(5);
 digitalWrite(trig1, LOW);
 
 float zeit_ms = pulseIn(echo1, HIGH); 
 float zeit_s = zeit_ms / 1000.0 / 1000.0 /2;
 float entfernung = zeit_s*schallgeschwindigkeit * 100;
 entfernung_ges = entfernung_ges + entfernung;
 }
 entfernung_in = entfernung_ges / 15;
 return entfernung_in;
}  

float entfernung2messen()
{
 float entfernung_ges = 0;
 float entfernung_in = 0;
 for(int x=0; x<15; x++)
 { 
 digitalWrite(trig2, LOW);
 delayMicroseconds(4);
 digitalWrite(trig2, HIGH);
 delayMicroseconds(5);
 digitalWrite(trig2, LOW);
 
 float zeit_ms = pulseIn(echo2, HIGH); 
 float zeit_s = zeit_ms / 1000.0 / 1000.0 /2;
 float entfernung = zeit_s*schallgeschwindigkeit * 100;
 entfernung_ges = entfernung_ges + entfernung;
 }
 entfernung_in = entfernung_ges / 15;
 return entfernung_in;
}  

float entfernung3messen()
{
 float entfernung_ges = 0;
 float entfernung_in = 0;
 for(int x=0; x<15; x++)
 { 
 digitalWrite(trig3, LOW);
 delayMicroseconds(4);
 digitalWrite(trig3, HIGH);
 delayMicroseconds(5);
 digitalWrite(trig3, LOW);
 
 float zeit_ms = pulseIn(echo3, HIGH); 
 float zeit_s = zeit_ms / 1000.0 / 1000.0 /2;
 float entfernung = zeit_s*schallgeschwindigkeit * 100;
 entfernung_ges = entfernung_ges + entfernung;
 }
 entfernung_in = entfernung_ges / 15;
 return entfernung_in;
}  

void vorwaertsfahren(int geschwindigkeit)
{
  digitalWrite(led_gruen, HIGH);
  
  digitalWrite(motor1_1, LOW);
  digitalWrite(motor1_2, HIGH);
  digitalWrite(motor2_1, LOW);
  digitalWrite(motor2_2, HIGH);
  digitalWrite(motor3_1, LOW);
  digitalWrite(motor3_2, HIGH);
  digitalWrite(motor4_1, LOW);
  digitalWrite(motor4_2, HIGH);
   
  analogWrite(motor1_speed, geschwindigkeit);
  analogWrite(motor2_speed, geschwindigkeit);
  analogWrite(motor3_speed, geschwindigkeit);
  analogWrite(motor4_speed, geschwindigkeit);
  
  delay(70);
  
  digitalWrite(motor1_speed, LOW);
  digitalWrite(motor2_speed, LOW);
  digitalWrite(motor3_speed, LOW);
  digitalWrite(motor4_speed, LOW); 
  
  digitalWrite(led_gruen, LOW);
  
}  

void bremsenr()
{
    digitalWrite(motor1_1, HIGH);
    digitalWrite(motor1_2, LOW);
    digitalWrite(motor2_1, HIGH);
    digitalWrite(motor2_2, LOW);
    digitalWrite(motor3_1, HIGH);
    digitalWrite(motor3_2, LOW);
    digitalWrite(motor4_1, HIGH);
    digitalWrite(motor4_2, LOW); 
 
    digitalWrite(motor1_speed, HIGH);
    digitalWrite(motor2_speed, HIGH);
    digitalWrite(motor3_speed, HIGH);
    digitalWrite(motor4_speed, HIGH); 
    
    delay(35);  
  
    digitalWrite(motor1_speed, LOW);
    digitalWrite(motor2_speed, LOW);
    digitalWrite(motor3_speed, LOW);
    digitalWrite(motor4_speed, LOW); 
}  

void bremsenv()
{
  digitalWrite(motor1_1, LOW);
  digitalWrite(motor1_2, HIGH);
  digitalWrite(motor2_1, LOW);
  digitalWrite(motor2_2, HIGH);
  digitalWrite(motor3_1, LOW);
  digitalWrite(motor3_2, HIGH);
  digitalWrite(motor4_1, LOW);
  digitalWrite(motor4_2, HIGH);
  
  digitalWrite(motor1_speed, HIGH);
  digitalWrite(motor2_speed, HIGH);
  digitalWrite(motor3_speed, HIGH);
  digitalWrite(motor4_speed, HIGH);   
  
  delay(35);  
  
  digitalWrite(motor1_speed, LOW);
  digitalWrite(motor2_speed, LOW);
  digitalWrite(motor3_speed, LOW);
  digitalWrite(motor4_speed, LOW); 
}