codArduino

#include <Servo.h>
int trigPin = 9;
int echoPin = 12;
int oprire = 0;
// Pinii motor 1
#define mpin00 5
#define mpin01 6
// Pinii motor 2
#define mpin10 3
#define mpin11 11
int distanta;
long duration;
Servo srv;
void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}
// Functie pentru controlul unui motor
// Intrare: pinii m1 si m2, directia si viteza
void StartMotor (int m1, int m2, int forward, int speed)
{

 if (speed==0) // oprire
 {
 digitalWrite(m1, 0);
 digitalWrite(m2, 0);
 }
 else
 {
 if (forward)
 {
 digitalWrite(m2, 0);
 analogWrite (m1, speed); // folosire PWM
 }
 else
 {
 digitalWrite(m1, 0);
 analogWrite(m2, speed);
 }
 }
}
// Functie de siguranta
// Executa oprire motoare, urmata de delay
void delayStopped(int ms)
{
 StartMotor (mpin00, mpin01, 0, 0);
 StartMotor (mpin10, mpin11, 0, 0);
 delay(ms);
}
// Utilizare servo
// Pozitionare in trei unghiuri
// La final, ramane in mijloc (90 grade)

void playWithServoC(int pin, long cm) {
  srv.attach(pin);
  srv.write(90);
  Serial.print("C");
  Serial.println(cm);
  delay(1500);
  srv.detach();
}

void playWithServoR(int pin, long cm) {
  srv.attach(pin);
  srv.write(0);
  Serial.print("R");
  Serial.println(cm);
  delay(1500);
  srv.detach();
}

void playWithServoL(int pin, long cm) {
  srv.attach(pin);
  srv.write(180);
  Serial.print("L");
  Serial.println(cm);
  delay(1500);
  srv.detach();
}
void playWithServo(int pin)
{
 srv.attach(pin);
 srv.write(0);
 delay(1000);
 srv.write(180);
 delay(1000);
 srv.write(90);
 delay(1000);
 srv.detach();
}

int distance(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  distanta = duration / 2 * 0.034;
  return distanta;
}
void loop() {
  int cm = distance();

  Serial.println(cm);
  delay(300);
}