Lenesul

/* Detectare obiect pe o linie si prindere cu gripper
    Acest program mentine robotul pe linie, detecteaza obiectul si il prinde cu gripperul.
     Folosim ca hardware mBot,senzor ultrasonic,senzor de linie si  gripper
*/
//includere biblioteca
#include "MeMCore.h"

//Definire constante
MeUltrasonicSensor senzorUltrasunete(PORT_4);
MeLineFollower senzorLinie(PORT_2);

MeDCMotor Motorstanga(M1);
MeDCMotor Motordreapta(M2);

//Definire variabile
int distanta;
int stare = 0;
//declarare pini pentru driver gripper
int m1p1 = 11; // pe pinul D10 Mcore - IN2 driver
int m1p2 = 12; // pe pinul D11 Mcore - IN1 driver

// Functii de miscare

void miscare(int VitezaStanga, int VitezaDreapta) {
  Motorstanga.run(VitezaStanga);
  Motordreapta.run(VitezaDreapta);
}
void oprire() {
  Motorstanga.run(0);
  Motordreapta.run(0);
}

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

void lineFollower() {
  //citire valoare senzor linie
  int linie = senzorLinie.readSensors();
  switch (linie)
  {
    case S1_IN_S2_IN:
      //afisare pe seriala:  "Senzorii 1 si 2 se afla pe linia neagra";
      miscare(100, 100 );
      stare = 1;
      break;
    case S1_IN_S2_OUT:
      //afisare pe seriala: "Senzorul 2 se afla inafara liniei negre"
      //miscare in fata
      stare = 2;
      break;
    case S1_OUT_S2_IN:
      //afisare pe seriala: "Senzorul 1 se afla inafara liniei negre"
      //miscare in fata
      stare = 3;
      break;
    case S1_OUT_S2_OUT:
      //afisare pe seriala: "Senzorii 1 si 2 se afla inafara liniei neagre"
      if (stare == 2) {
        //viraj la stanga
      }
      if (stare == 3) {
        //viraj la dreapta
      }
      if (stare == 1) {
        //rotire
      }
      break;
    default: break;
  }
}


void setup() {
  // acest cod este executat o singura data la pornire
  //setare pini driver gripper ca pini de iesire
  //definire consola seriala
  deschide(); //apelare functie deschidere gripper
  delay(500);
  stai(); //apelare functie oprire gripper
}
void loop() {
  //acest cod este executat si repetat la infinit
  distanta = senzorUltrasunete.distanceCm(); //citire valoare senzor ultrasonic
  if (distanta < 7) {
    //oprire mBot
    //inchidere gripper
    delay(500);
    //oprire gripper
    }
    else
      lineFollower();

  }