854 lines
21 KiB
C++
854 lines
21 KiB
C++
/*Programar como Arduino Pro Mini con CPU ATMEGA168 a 3.3v y 8MHz*/
|
|
#include <Wire.h>
|
|
#include "HMC6352.h"
|
|
#include <Servo.h>
|
|
//#include <SoftwareServo.h>
|
|
|
|
// TONES ==========================================
|
|
// Start by defining the relationship between
|
|
// note, period, & frequency.
|
|
#define c 3830 // 261 Hz
|
|
#define d 3400 // 294 Hz
|
|
#define e 3038 // 329 Hz
|
|
#define f 2864 // 349 Hz
|
|
#define g 2550 // 392 Hz
|
|
#define a 2272 // 440 Hz
|
|
#define b 2028 // 493 Hz
|
|
#define C 1912 // 523 Hz
|
|
// Define a special note, 'R', to represent a rest
|
|
#define R 0
|
|
|
|
boolean demo = true;
|
|
|
|
int melodyStart[] = { C, b, g, C, R};
|
|
//int melodyStart[] = { c, d, e, f, g, a, b, C, R };
|
|
int beatsStart[] = { 16, 16, 16, 8, 32};
|
|
|
|
int melodyStop[] = { b, e};
|
|
int beatsStop[] = { 8, 16};
|
|
|
|
int melodyNo[] = { c };
|
|
int beatsNo[] = { 32};
|
|
|
|
int MAX_COUNT1 = sizeof(melodyStart)/2; // Melody length, for looping.
|
|
int MAX_COUNT2 = sizeof(melodyStop)/2; // Melody length, for looping.
|
|
int MAX_COUNT3 = sizeof(melodyNo)/2; // Melody length, for looping.
|
|
|
|
// Set overall tempo
|
|
long tempo = 10000;
|
|
// Set length of pause between notes
|
|
int pause = 1000;
|
|
// Loop variable to increase Rest length
|
|
int rest_count = 100; //<-BLETCHEROUS HACK; See NOTES
|
|
|
|
// Initialize core variables
|
|
int tono = 0;
|
|
int beat = 0;
|
|
long duration = 0;
|
|
|
|
|
|
//SoftwareServo servoA;
|
|
//SoftwareServo servoB;
|
|
|
|
Servo servoA;
|
|
Servo servoB;
|
|
|
|
//Control Shift register
|
|
#define latchPin 13
|
|
#define clockPin 12
|
|
#define dataPin 11
|
|
|
|
//Salidas Shift register
|
|
#define IR 0 //Resistencia 56 Ohm
|
|
int idpins[4] = { 5, 6, 7, 8 };
|
|
#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 //Nivel minimo de bateria
|
|
#define max_level 984 //Nivel maximo de bateria
|
|
|
|
#define MAX_HOR 180 //MAXIMO IZQUIERDA
|
|
#define MED_HOR 90 //MEDIO
|
|
#define MIN_HOR 0 //MAXIMO DERECHA
|
|
|
|
#define MIN_VER 85 //MAXIMO ABAJO
|
|
#define MED_VER 97 //MEDIO
|
|
#define MAX_VER 120 //MAXIMO ARRIBA
|
|
|
|
float distancia=150; //Distancia deteccion ultrasonido
|
|
|
|
//Entradas digitales
|
|
#define CTS 4
|
|
|
|
//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
|
|
|
|
float angulo = 0;
|
|
float angulo_ant = 0;
|
|
float angulo_act = 0;
|
|
float angulo_neg = 0;
|
|
|
|
byte val = 0x00; //Valor entrada serie
|
|
int n_bat = 0;
|
|
|
|
int Command_ok=0;
|
|
int mensaje_ok=0;
|
|
|
|
int modo=0;
|
|
byte dat = 0;
|
|
int command = 0;
|
|
int count = 0;
|
|
|
|
int frec = 0;
|
|
int activa = 0;
|
|
boolean flag_giro = false;
|
|
|
|
unsigned long time=0;
|
|
unsigned long time_correc=0;
|
|
|
|
int datd = 0;
|
|
int dati = 0;
|
|
int pos = 90;
|
|
|
|
int inc = 1;
|
|
int neg_sig=0;
|
|
int pos_sig=0;
|
|
|
|
int flag_avanza_stop=1;
|
|
int flag_avanza=1;
|
|
|
|
int IR1=0;
|
|
|
|
boolean camera_on = false;
|
|
|
|
boolean scan_mode = false;
|
|
|
|
#define time_robot_max 10000
|
|
unsigned long time_robot_count = 0;
|
|
|
|
// defines for setting and clearing register bits
|
|
#ifndef cbi
|
|
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
|
|
#endif
|
|
#ifndef sbi
|
|
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
|
|
#endif
|
|
|
|
void setup()
|
|
{
|
|
pinMode(latchPin, OUTPUT);
|
|
pinMode(clockPin, OUTPUT);
|
|
pinMode(dataPin, OUTPUT);
|
|
servoA.attach(2);
|
|
servoB.attach(3);
|
|
stop_motores();
|
|
analogWrite(speed1, 0);
|
|
analogWrite(speed2, 0);
|
|
Wire.begin();
|
|
Serial.begin(19200);
|
|
|
|
pinMode(CTS, INPUT); //Pin de control de flujo de datos
|
|
|
|
//Lectura del identificador de la placa
|
|
for (int i=0; i<4; i++) pinMode(idpins[i], INPUT);
|
|
|
|
digital_shift(servo_on, LOW);
|
|
servoA.write(MED_VER); //Vertical
|
|
servoB.write(MED_HOR); //Horizontal
|
|
delay(15);
|
|
|
|
if (id()<2)
|
|
{
|
|
digital_shift(camara_on, HIGH);
|
|
camera_on = true;
|
|
}
|
|
else digital_shift(camara_on, LOW);
|
|
delay(4000);
|
|
while(digitalRead(CTS));
|
|
Serial.print("Soy el robot ");
|
|
Serial.println(id());
|
|
delay(1000);
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
angulo=0;
|
|
time_correc=millis();
|
|
pitidoPositivo();
|
|
time_robot_count = millis();
|
|
//if (demo) maximun_gyro();
|
|
}
|
|
|
|
float limite_min = 0;
|
|
float limite_max = 0;
|
|
|
|
void maximun_gyro()
|
|
{
|
|
float limite_izquierdo = 0;
|
|
float limite_derecho = 0;
|
|
giro_der(255,255);
|
|
delay(1000);
|
|
Serial.println("Calibrando derecha...");
|
|
HMC6352.Wake();
|
|
float angulo_int = HMC6352.GetHeading();
|
|
float angulo_int_ant = 0;
|
|
while (angulo_int!=angulo_int_ant)
|
|
{
|
|
angulo_int_ant = angulo_int;
|
|
angulo_int = HMC6352.GetHeading();
|
|
delay(100);
|
|
}
|
|
stop_motores();
|
|
delay(1000);
|
|
limite_derecho = angulo_int;
|
|
Serial.println(limite_derecho);
|
|
giro_iz(255,255);
|
|
delay(1000);
|
|
angulo_int = HMC6352.GetHeading();
|
|
Serial.println("Calibrando izquierda...");
|
|
while (angulo_int!=angulo_int_ant)
|
|
{
|
|
angulo_int_ant = angulo_int;
|
|
angulo_int = HMC6352.GetHeading();
|
|
delay(100);
|
|
}
|
|
stop_motores();
|
|
delay(1000);
|
|
limite_izquierdo = angulo_int;
|
|
Serial.println(limite_izquierdo);
|
|
if (limite_derecho>limite_izquierdo) {limite_max=limite_derecho; limite_min=limite_izquierdo;}
|
|
else {limite_max=limite_izquierdo; limite_min=limite_derecho;}
|
|
angulo = (limite_max - limite_min)/2 + limite_min;
|
|
Serial.println("Centrando...");
|
|
Serial.println(angulo);
|
|
flag_giro=1;
|
|
while (flag_giro==1) correccion_ang(); //Giro del robot
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
Serial.println(angulo_act);
|
|
//HMC6352.Sleep();
|
|
}
|
|
|
|
unsigned long time_scan = 0;
|
|
|
|
void loop() //run over and over again
|
|
{
|
|
if (demo) scan();
|
|
if (flag_giro==1) correccion_ang(); //Giro del robot
|
|
if ((measure(sensor1)<=19)&&(flag_avanza_stop==1))//Para ULTRASONIDO
|
|
{
|
|
flag_avanza_stop=0;
|
|
stop_motores();
|
|
pitidoParada();
|
|
}
|
|
else if (analogRead(sensor1)>50) flag_avanza_stop=1;
|
|
Serial_read();
|
|
|
|
|
|
}
|
|
|
|
void Serial_read()
|
|
{
|
|
if (Serial.available()) {
|
|
val = Serial.read();
|
|
if ((val&0xF0)==0xF0)
|
|
{
|
|
mensaje_ok=1;
|
|
count=1; //OTRO ID PUEDE RESETEAR EL ACTUAL...
|
|
command=0x00;
|
|
dat=0x00;
|
|
modo=val;
|
|
}
|
|
else
|
|
{
|
|
switch (modo) {
|
|
case 0xF0:
|
|
if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==1))
|
|
{
|
|
command=val&0x0F;
|
|
count=2; // SE INCREMENTA EL CONTADOR DE BYTES SOLO SI EL ID ES EL CORRECTO
|
|
}
|
|
else if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==2))
|
|
{
|
|
dat = (val&0x0F)<<4;
|
|
count=3;
|
|
}
|
|
else if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==3))
|
|
{
|
|
dat = dat|(val&0x0F);
|
|
count=4;
|
|
//Serial.print(0xF3, BYTE);
|
|
}
|
|
break;
|
|
case 0xF2:
|
|
//if (((val>>4)==id())&&(count==1))
|
|
if (count==1)
|
|
{
|
|
count=0;
|
|
time_robot_count = millis();
|
|
//Serial.print(0xF4, BYTE);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
} // ENDIF DEL AVAILABLESERIAL
|
|
|
|
if ((count==4)&&(mensaje_ok==1)) // MENSAJE RECIBIDO, VAMOS A PROCESAR EL COMANDO
|
|
{
|
|
count=0;
|
|
mensaje_ok = 0;
|
|
//Serial.print(command, BYTE);
|
|
//Serial.print(dat, BYTE);
|
|
|
|
switch (command) {
|
|
|
|
case 0x00: //Velocidad hacia delante
|
|
if (flag_avanza_stop==1) avanza(dat);
|
|
break;
|
|
case 0x01: //Velocidad hacia atras
|
|
retrocede(dat);
|
|
break;
|
|
case 0x02: //GIRO ROBOT ABSOLUTO
|
|
{
|
|
angulo_ant = angulo;
|
|
angulo=map(dat, 0, 180, 0, 359);
|
|
if ((angulo_ant==angulo)&&(flag_giro==0)) flag_giro=0;
|
|
else flag_giro=1;
|
|
//Serial.print(angulo);
|
|
//Serial.print(angulo_ant);
|
|
}
|
|
break;
|
|
case 0x03: //CAMARA ON/OFF
|
|
{
|
|
if (dat==0x01)
|
|
{
|
|
digital_shift(camara_on, HIGH);
|
|
camera_on = true;
|
|
}
|
|
else if (dat==0x00)
|
|
{
|
|
digital_shift(camara_on, LOW);
|
|
camera_on = false;
|
|
}
|
|
}
|
|
break;
|
|
case 0x04://Calibracion Brujula
|
|
{
|
|
if (dat==0x01)
|
|
{
|
|
HMC6352.startcal() ;
|
|
giro_der(255,255);
|
|
Serial.println("Calibrando...");
|
|
delay(15000);
|
|
HMC6352.stopcal() ;
|
|
stop_motores();
|
|
Serial.println("Fin de la calibracion.");
|
|
}
|
|
else if (dat==0x00)
|
|
{
|
|
HMC6352.stopcal() ;
|
|
stop_motores();
|
|
Serial.println("Fin de la calibracion.");
|
|
}
|
|
}
|
|
break;
|
|
case 0x05: //ZUMBADOR
|
|
{
|
|
if (dat==0x00) pitidoParada();
|
|
else if (dat==0x01) pitidoNegativo();
|
|
}
|
|
break;
|
|
case 0x06: //IR
|
|
{
|
|
if (dat==0x01) digital_shift(IR, HIGH);
|
|
else digital_shift(IR, LOW);
|
|
|
|
n_bat=analogRead(vbat);
|
|
n_bat=map(n_bat,min_level,max_level,0,100);
|
|
IR1=analogRead(sensor1);
|
|
IR1=map(IR1,0,1023,0,255);
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
byte id_temp = id();
|
|
while(digitalRead(CTS));
|
|
Serial.write(0xF1);
|
|
while(digitalRead(CTS));
|
|
Serial.write(id_temp<<4|0x00);
|
|
while(digitalRead(CTS));
|
|
Serial.write(id_temp<<4|((int (angulo_act/2))>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write(id_temp<<4|((int (angulo_act/2))&0x0F));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id_temp<<4)|(n_bat>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id_temp<<4)|(n_bat&0x0F));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id_temp<<4)|(IR1>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id_temp<<4)|(IR1&0x0F));
|
|
}
|
|
break;
|
|
case 0x07: //GIRO ROBOT RELATIVO
|
|
{
|
|
// if (dat<=127) giro_iz(map(dat,0,127,0,255), map(dat,0,127,0,255));
|
|
// else if (dat>=129) giro_der(map(dat,129,255,0,255), map(dat,129,255,0,255));
|
|
if (dat<=127) giro_iz(255, 255);
|
|
else if (dat>=129) giro_der(255, 255);
|
|
else stop_motores();
|
|
}
|
|
break;
|
|
case 0x08: //Stop Motores
|
|
{
|
|
stop_motores();
|
|
}
|
|
break;
|
|
case 0x09: //Posicion Inicial Cuello
|
|
{
|
|
digital_shift(servo_on, LOW);
|
|
servoA.write(60);
|
|
servoB.write(90);
|
|
delay(15);
|
|
}
|
|
break;
|
|
case 0x0A: //Giro cuello vertical
|
|
{
|
|
digital_shift(servo_on, LOW);
|
|
//servoA.write(map(dat, 0, 180, 0, 114)+66);
|
|
if (dat>180) dat=180;
|
|
servoA.write(map(dat, 0, 180, MAX_VER, MIN_VER));
|
|
delay(15);
|
|
}
|
|
break;
|
|
case 0x0B: //Giro cuello Horizontal
|
|
{
|
|
digital_shift(servo_on, LOW);
|
|
if (dat>180) dat=180;
|
|
servoB.write(map(dat, 0, 180, MIN_HOR, MAX_HOR));
|
|
//servoB.write(dat);
|
|
delay(15);
|
|
}
|
|
break;
|
|
case 0x0C: //Apagar servos
|
|
{
|
|
digital_shift(servo_on, HIGH);
|
|
}
|
|
break;
|
|
case 0x0D: //Apagar servos
|
|
{
|
|
scan_mode=true;
|
|
time_scan = millis();
|
|
}
|
|
break;
|
|
case 0x0E: //Apagar servos
|
|
{
|
|
demo=false;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
//Comando de vuelta
|
|
}
|
|
}
|
|
|
|
void digital_shift(int pin,int state)
|
|
{
|
|
switch (pin) {
|
|
case 0:
|
|
if (state==1) val_shift= B00000001|val_shift;
|
|
else val_shift= B11111110&val_shift;
|
|
break;
|
|
case 1:
|
|
if (state==1) val_shift= B00000010|val_shift;
|
|
else val_shift= B11111101&val_shift;
|
|
break;
|
|
case 2:
|
|
if (state==1) val_shift= B00000100|val_shift;
|
|
else val_shift= B11111011&val_shift;
|
|
break;
|
|
case 3:
|
|
if (state==1) val_shift= B00001000|val_shift;
|
|
else val_shift= B11110111&val_shift;
|
|
break;
|
|
case 4:
|
|
if (state==1) val_shift= B00010000|val_shift;
|
|
else val_shift= B11101111&val_shift;
|
|
break;
|
|
case 5:
|
|
if (state==1) val_shift= B00100000|val_shift;
|
|
else val_shift= B11011111&val_shift;
|
|
break;
|
|
case 6:
|
|
if (state==1) val_shift= B01000000|val_shift;
|
|
else val_shift= B10111111&val_shift;
|
|
break;
|
|
case 7:
|
|
if (state==1) val_shift= B10000000|val_shift;
|
|
else val_shift= B01111111&val_shift;
|
|
break;
|
|
}
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
|
|
}
|
|
|
|
void stop_motores()
|
|
{
|
|
val_shift= B11100001&val_shift;
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
analogWrite(speed1, 0);
|
|
analogWrite(speed2, 0);
|
|
}
|
|
|
|
void giro_iz(int iz, int der)
|
|
{
|
|
val_shift= B11101101&val_shift;
|
|
val_shift= B00001100|val_shift;
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
analogWrite(speed1, iz);
|
|
analogWrite(speed2, der);
|
|
}
|
|
|
|
void giro_der(int iz, int der)
|
|
{
|
|
val_shift= B1110011&val_shift;
|
|
val_shift= B0010010|val_shift;
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
analogWrite(speed1, iz);
|
|
analogWrite(speed2, der);
|
|
}
|
|
|
|
void avanza(int av)
|
|
{
|
|
//stop_motores();
|
|
val_shift= B11101011&val_shift;
|
|
val_shift= B00001010|val_shift;
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
analogWrite(speed2, av);
|
|
analogWrite(speed1, av);
|
|
}
|
|
|
|
void retrocede(int re)
|
|
{
|
|
//stop_motores();
|
|
val_shift= B11110101&val_shift;
|
|
val_shift= B00010100|val_shift;
|
|
digitalWrite(latchPin, 0);
|
|
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
|
|
digitalWrite(latchPin, 1);
|
|
analogWrite(speed2, re);
|
|
analogWrite(speed1, re);
|
|
}
|
|
|
|
void correccion_ang( )
|
|
{
|
|
#define res 3
|
|
int sentido_giro=0;
|
|
|
|
digital_shift(IR, LOW);
|
|
|
|
if ((millis()-time_correc)>=15)
|
|
{
|
|
angulo_act=0;
|
|
time_correc=millis();
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
//Serial.println(angulo_act);
|
|
if ((angulo<angulo_act)&&((angulo_act-angulo)>180)) angulo_neg=angulo+360;
|
|
else angulo_neg=angulo;
|
|
|
|
if (((abs(angulo_act-angulo)<abs(360-angulo+angulo_act))&&(angulo_act<=angulo))||((abs(360-angulo_act+angulo)<abs(angulo_act-angulo))&&(angulo_act>angulo))) sentido_giro=1;
|
|
else sentido_giro=0;
|
|
|
|
if (abs(angulo_act-angulo_neg)<=res)
|
|
{
|
|
flag_giro=0;
|
|
stop_motores();
|
|
}
|
|
else
|
|
{
|
|
if (sentido_giro==0) giro_iz(255,255);
|
|
else giro_der(255,255);
|
|
delay(20);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
// PLAY TONE ==============================================
|
|
// Pulse the speaker to play a tone for a particular duration
|
|
void playTone() {
|
|
long elapsed_time = 0;
|
|
if (tono > 0) { // if this isn't a Rest beat, while the tone has
|
|
// played less long than 'duration', pulse speaker HIGH and LOW
|
|
while (elapsed_time < duration) {
|
|
|
|
digital_shift(buzzer,HIGH);
|
|
delayMicroseconds(tono / 2);
|
|
|
|
// DOWN
|
|
digital_shift(buzzer,LOW);
|
|
delayMicroseconds(tono / 2);
|
|
|
|
// Keep track of how long we pulsed
|
|
elapsed_time += (tono);
|
|
}
|
|
}
|
|
else { // Rest beat; loop times delay
|
|
for (int j = 0; j < rest_count; j++) { // See NOTE on rest_count
|
|
delayMicroseconds(duration);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void pitidoPositivo() {
|
|
|
|
for (int i=0; i<MAX_COUNT1; i++) {
|
|
tono = melodyStart[i];
|
|
beat = beatsStart[i];
|
|
|
|
duration = beat * tempo; // Set up timing
|
|
playTone();
|
|
// A pause between notes...
|
|
delayMicroseconds(pause);
|
|
}
|
|
}
|
|
|
|
void pitidoParada(){
|
|
|
|
for (int i=0; i<MAX_COUNT2; i++) {
|
|
tono = melodyStop[i];
|
|
beat = beatsStop[i];
|
|
|
|
duration = beat * tempo; // Set up timing
|
|
playTone();
|
|
// A pause between notes...
|
|
delayMicroseconds(pause);
|
|
}
|
|
|
|
}
|
|
|
|
void pitidoNegativo() {
|
|
|
|
for (int i=0; i<MAX_COUNT3; i++) {
|
|
tono = melodyNo[i];
|
|
beat = beatsNo[i];
|
|
|
|
duration = beat * tempo; // Set up timing
|
|
playTone();
|
|
// A pause between notes...
|
|
delayMicroseconds(pause);
|
|
}
|
|
}
|
|
|
|
int id()
|
|
{
|
|
int id = 0;
|
|
for(int i=0; i<4; i++)
|
|
for (byte i=0; i<4; i++) bitWrite(id, i, !digitalRead(idpins[i]));
|
|
return id;
|
|
}
|
|
|
|
#define RIGHT true
|
|
#define LEFT false
|
|
|
|
boolean turn = RIGHT;
|
|
int angle = MIN_HOR;
|
|
int angle_ant = 0;
|
|
|
|
boolean message_send0 = true;
|
|
boolean message_send = true;
|
|
|
|
void scan()
|
|
{
|
|
|
|
if ((millis()-time_robot_count)> time_robot_max)
|
|
{
|
|
servoA.write(MED_VER); //Vertical
|
|
servoB.write(MED_HOR); //Horizontal
|
|
delay(15);
|
|
for (int i=0; i<100; i++)
|
|
{
|
|
Serial_read();
|
|
delay(10);
|
|
}
|
|
digital_shift(servo_on, HIGH);
|
|
float prox = measure(sensor1);
|
|
if (prox <= distancia) time_robot_count = millis();
|
|
if (id()==2) send_message(0xF0, 0x03, 0x00, 0x01);
|
|
else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x11);
|
|
}
|
|
else
|
|
{
|
|
digital_shift(servo_on, LOW);
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
float prox = measure(sensor1);
|
|
if (message_send0)
|
|
{
|
|
message_send0 = false;
|
|
if (id()==2) send_message(0xF0, 0x03, 0x00, 0x01);
|
|
else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x11);
|
|
}
|
|
//Serial.println(prox);
|
|
if (turn == RIGHT)
|
|
{
|
|
servoB.write(angle); //Horizontal
|
|
delay(15);
|
|
if (camera_on==false) digital_shift(camara_on, LOW);
|
|
else digital_shift(camara_on, HIGH);
|
|
while (prox <= distancia)
|
|
{
|
|
message_send0 = true;
|
|
send_message();
|
|
delay(500);
|
|
time_robot_count = millis();
|
|
prox = measure(sensor1);
|
|
Serial_read();
|
|
if (((id()<2)&&(camera_on))||(id()>=2))
|
|
{
|
|
if (message_send)
|
|
{
|
|
message_send = false;
|
|
if (id()==2) send_message(0xF0, 0x03, 0x00, 0x00);
|
|
else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x10);
|
|
}
|
|
digital_shift(camara_on, HIGH);
|
|
}
|
|
delay(500);
|
|
//if (angle!=MED_HOR) correccion_cuerpo();
|
|
}
|
|
message_send = true;
|
|
angle++;
|
|
if (angle>=MAX_HOR) turn = LEFT;
|
|
}
|
|
else if (turn == LEFT)
|
|
{
|
|
servoB.write(angle); //Horizontal
|
|
delay(15);
|
|
if (camera_on==false) digital_shift(camara_on, LOW);
|
|
else digital_shift(camara_on, HIGH);
|
|
while (prox <= distancia)
|
|
{
|
|
message_send0 = true;
|
|
send_message();
|
|
delay(500);
|
|
time_robot_count = millis();
|
|
prox = measure(sensor1);
|
|
Serial_read();
|
|
if (((id()<2)&&(camera_on))||(id()>=2))
|
|
{
|
|
if (message_send)
|
|
{
|
|
message_send = false;
|
|
if (id()==2) send_message(0xF0, 0x03, 0x00, 0x00);
|
|
else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x10);
|
|
}
|
|
digital_shift(camara_on, HIGH);
|
|
}
|
|
delay(500);
|
|
//if (angle!=MED_HOR) correccion_cuerpo();
|
|
}
|
|
message_send = true;
|
|
angle--;
|
|
if (angle<=MIN_HOR) turn = RIGHT;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void send_message(byte val0, byte val1, byte val2, byte val3)
|
|
{
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
while(digitalRead(CTS));
|
|
Serial.write(val0);
|
|
while(digitalRead(CTS));
|
|
Serial.write(val1);
|
|
while(digitalRead(CTS));
|
|
Serial.write(val2);
|
|
while(digitalRead(CTS));
|
|
Serial.write(val3);
|
|
}
|
|
}
|
|
|
|
void send_message()
|
|
{
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
while(digitalRead(CTS));
|
|
Serial.write(0xF2);
|
|
while(digitalRead(CTS));
|
|
Serial.write(0x00);
|
|
}
|
|
}
|
|
|
|
|
|
boolean correccion_cuerpo()
|
|
{
|
|
int angle_turn = MED_HOR - angle;
|
|
angulo = angulo_act - angle_turn;
|
|
Serial.print(angulo);
|
|
Serial.print(" ");
|
|
if (angulo<0) angulo = 360 + angulo;
|
|
if (angulo<limite_min) angulo = limite_min;
|
|
else if (angulo>limite_max) angulo = limite_max;
|
|
Serial.println(angulo);
|
|
digital_shift(camara_on, HIGH);//Para ULTRASONIDO
|
|
flag_giro = true;
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
if (angulo!=angulo_act)
|
|
{
|
|
angle = MED_HOR;
|
|
servoB.write(angle); //Horizontal
|
|
delay(15);
|
|
while (flag_giro==1)
|
|
{
|
|
correccion_ang(); //Giro del robot
|
|
}
|
|
}
|
|
}
|
|
|
|
float num = 10;
|
|
float measure(int sensor)
|
|
{
|
|
float value = 0;
|
|
for (int i=0; i<num; i++) value = value + analogRead(sensor);
|
|
value= value/num;
|
|
float voltage = value*(5000/1023.);
|
|
float cm = voltage*2.54/9.8;
|
|
return cm;
|
|
}
|
|
|