#define trigPin 13 //Trig Ultrasons (output) #define echoPin 12 //Echo Ultrasons (input) #define ledGreen 11 // green LED #define ledYellow 10 // yellow LED #define mRight 7 // #define mLeft 6 #define mForward 5 #define mBackward 4 #define test 3 #define tests 2 char command; void setup() { Serial.begin (9600); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(ledGreen, OUTPUT); pinMode(ledYellow, OUTPUT); pinMode(mForward, OUTPUT); pinMode(mBackward, OUTPUT); pinMode(mRight, OUTPUT); pinMode(mLeft, OUTPUT); pinMode(test, OUTPUT); pinMode(tests, OUTPUT); } void loop() { long duration, distance; digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); // echo calculating duration = pulseIn(echoPin, HIGH); // proportional distance to output delay distance = duration*340/(2*10000); //theorical sound speed if (distance < 20) { // too close digitalWrite(ledYellow,HIGH); digitalWrite(ledGreen,LOW); digitalWrite(mForward, LOW); } else { // far enough digitalWrite(ledYellow,LOW); digitalWrite(ledGreen,HIGH); } if(Serial.available() > 0){ command = Serial.read(); Stop(); //initialize with motors stoped //Change pin mode only if new command is different from previous. //Serial.println(command); switch(command){ case 'F': forward(); break; case 'G': forward(); left(); break; case 'I': forward(); right(); break; case 'B': backward(); break; case 'H': backward(); left(); break; case 'J': backward(); right(); break; case 'L': left(); break; case 'R': right(); break; case 'S': Stop(); break; } } } void forward() { digitalWrite(tests, HIGH); digitalWrite(mForward, HIGH); digitalWrite(mBackward, LOW); } void backward() { digitalWrite(tests, HIGH); digitalWrite(mBackward, HIGH); digitalWrite(mForward, LOW); } void right() { digitalWrite(test, HIGH); digitalWrite(mRight, HIGH); digitalWrite(mLeft, LOW); } void left() { digitalWrite(test, HIGH); digitalWrite(mLeft, HIGH); digitalWrite(mRight, LOW); } void Stop() { digitalWrite(mForward, LOW); digitalWrite(mBackward, LOW); digitalWrite(mRight, LOW); digitalWrite(mLeft, LOW); }