#include <Wire.h> 
#include <hmc6352.h>
#include <SoftwareServo.h>

SoftwareServo servoA;
SoftwareServo servoB;

Hmc6352 hmc6352;

//Control Shift register
#define latchPin 13
#define clockPin 12
#define dataPin 11

//Salidas Shift register
#define IR 0  //Resistencia 56 Ohm
#define INPUT1 1
#define INPUT2 2
#define INPUT3 3
#define INPUT4 4
#define servo_on 5
#define camara_on 6
#define buzzer 7

#define min_level 702
#define  max_level 984
  
//Entradas digitales
#define CTS 4
#define bit0 5
#define bit1 6
#define bit2 7
#define bit3 8

//Salidas digitales
#define speed1 10
#define speed2 9

//Entradas analogicas
#define vbat 3
#define sensor1 7
#define sensor2 0
#define sensor3 1
#define sensor4 2


int val_shift = 0x00; //Valor del shift register

int id = 0; //Identificador de la placa
int id0 = 0;
int id1 = 0;
int id2 = 0;
int id3 = 0;

float angulo = 0;
float angulo_act = 0;
float angulo_ant = 0;
float angulo_neg = 0;
float angulo_pos = 0;

int val = 0x00; //Valor entrada serie
int n_bat = 0;

int Command_ok=0;
int mensaje_ok=0;
int dat = 0;
int command = 0;
int count = 0;

int frec = 0;
int activa = 0;
int flag_giro=1;
int stop_flag=0;

unsigned long time=0;
unsigned long time_ant=0;
unsigned long time_max=0;
unsigned long time_correc=0;
unsigned long time_stop=0;
unsigned long time_servo=0;
int datd = 0;
int dati = 0;
int pos = 90;

int inc = 1;
int neg_sig=0;
int pos_sig=0;


void setup()                    
{
 Serial.begin(19200); 
}

void loop()                      //run over and over again
{


  n_bat=analogRead(vbat);
  Serial.print(n_bat);

}