/* Miscare robot si control gripper cu ajutorul telecomenzii
   Vom misca mBotul si vom deschide/inchide gripperul  prin intermediul telecomenzii
*/
//includere biblioteci
#include "MeMCore.h"
#include "Wire.h"
//declarare motoare

//definire receptor infrarosu (IR)

//declarare variabile
uint32_t value = 0x00;
int m1p1 = 11; // pe pinul D10 Mcore - IN2 driver
int m1p2 = 12; // pe pinul D11 Mcore - IN1 driver

//Functii miscare, inchidere,deschidere si oprire gripper

void miscare(int VitezaStanga, int VitezaDreapta) //functie pentru miscarea motoarelor robotului
{
  MotorStanga.run(-VitezaStanga);
  MotorDreapta.run(VitezaDreapta);
}
void inchide()
{ //inchidere gripper
  digitalWrite(m1p1, HIGH);
  digitalWrite(m1p2, LOW);
  delay(50);
}


void deschide() {
  //deschidere gripper
  digitalWrite(m1p2, HIGH);
  digitalWrite(m1p1, LOW);
  delay(50);
}
void stai() {
  //oprire gripper
  digitalWrite(m1p1, LOW);
  digitalWrite(m1p2, LOW);
}
void setup() {
  //initializare receptor IR
  //initializare consola seriala
  //setare pini driver gripper ca pini de iesire
  //oprire gripper
  //pauza 500ms
}
void loop() {
  if (receptor.decode()) {
    value = receptor.value >> 16 & 0xFF; //decodarea semnalului transmis de telecomanda
  } else value = 0x00;
  switch (value) {
    //afisam pe consola seriala ce tasta a fost apasata si miscam mBotul
    case 0x40: //afisare pe consola seriala mesajul inainte
      //miscare robot inainte
      delay(200); break;
    case 0x7: //afisare pe consola seriala mesajul stanga
      //miscare robot stanga
      delay(200); break;
    case 0x9:  //afisare pe consola seriala mesajul dreapta
      //miscare robot dreapta
      delay(200); break;
    case 0x19://afisare pe consola seriala mesajul inapoi
      //miscare robot inapoi
      delay(200); break;
    case 0x00: //oprire robot
      //oprire gripper
      break;  // la apasare tasta 1
    case 0x45: //afisare pe consola seriala mesajul deschidere gripper
      //deschiderea gripperului
      delay(200); break; //tasta A
    case 0x46: //afisare pe consola seriala mesajul inchidere gripper
      //inchiderea gripperului
      delay(200); break; //tasta B
    default: //oprire gripper
      break;
  }
}