/*
 * mBotul trebuie sa primeasca un obiect in gripper si sa il duca intr-o zona stabilita.
 * Pentru asta il vom programa sa ia obiectul (la inceputul programului - in setup) si sa mearga in fata pana vede un obstacol.
 * Cand inalneste un obstacol, lasa obiectul jos.
 */

#include "MeMCore.h"
int m1p1 = 11; //portul 1 de pe mBot
int m1p2 = 12;

MeDCMotor MotorStanga(M1); //declarare motoare
MeDCMotor MotorDreapta(M2);

MeUltrasonicSensor SenzorDistanta(PORT_2);

int distanta;
int val = 0;

void inchidere()
{
  digitalWrite(m1p1, HIGH);
  digitalWrite(m1p2, LOW);
  delay(50);
}

void deschidere()
{
  digitalWrite(m1p2, HIGH);
  digitalWrite(m1p1, LOW);
  delay(50);
}

void stai()
{
  digitalWrite(m1p2, LOW);
  digitalWrite(m1p1, LOW);
  delay(50);
}

void setup() {
  //acest cod este executat o singura data
  pinMode(m1p1, OUTPUT);
  pinMode(m1p2, OUTPUT);
  Serial.begin(9600);
  Serial.println("Start");
  //deschidere gripper timp de 1 secunda
  //inchidere gripper timp de 1 secunda
  //oprire gripper
  delay(3000); //ai timp 3 secunde sa iei mana din fata senzorului de distanta
}

void miscare(int stanga, int dreapta)
{
  MotorStanga.run(-stanga);
  MotorDreapta.run(dreapta);
}

void loop() {
  //acest cod este executat la infinit

  //citire valoare senzor de distanta in variabila distanta
  if ((distanta < 10) && (val == 0)) //daca distanta este < 10 si nu a lasat obiectul, se opreste si lasa obiectul
  {
    //oprire motoare
    //deschidere gripper timp de 2 secunde
    //oprire gripper
    val = 1; //lasa obiectul
  }
  if ((distanta > 11) && (val == 0)) //daca nu are obstacol in fata si nu a lasat obiectul din gripper
    //merge in fata
}