593 lines
14 KiB
C++
593 lines
14 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
|
|
|
|
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
|
|
#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
|
|
|
|
//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 mac = 0; //Identificador de la placa
|
|
|
|
int id=0;
|
|
|
|
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;
|
|
int flag_giro=1;
|
|
|
|
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;
|
|
|
|
// 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()
|
|
{
|
|
// // set prescale to 16
|
|
// sbi(ADCSRA,ADPS2) ;
|
|
// cbi(ADCSRA,ADPS1) ;
|
|
// cbi(ADCSRA,ADPS0) ;
|
|
pinMode(latchPin, OUTPUT);
|
|
pinMode(clockPin, OUTPUT);
|
|
pinMode(dataPin, OUTPUT);
|
|
stop_motores();
|
|
|
|
servoA.attach(2);
|
|
// servoA.writeMicroseconds(1500);
|
|
servoB.attach(3);
|
|
// servoB.writeMicroseconds(1500);
|
|
|
|
|
|
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
|
|
pinMode(bit0, INPUT);
|
|
pinMode(bit1, INPUT);
|
|
pinMode(bit2, INPUT);
|
|
pinMode(bit3, INPUT);
|
|
|
|
if (digitalRead(bit0)) mac=1;
|
|
if (digitalRead(bit1)) mac=mac+2;
|
|
if (digitalRead(bit2)) mac=mac+4;
|
|
if (digitalRead(bit3)) mac=mac+8;
|
|
|
|
id=mac;
|
|
|
|
digital_shift(servo_on,LOW);
|
|
servoA.write(MED_VER); //Vertical
|
|
servoB.write(MED_HOR); //Horizontal
|
|
delay(15);
|
|
|
|
digital_shift(camara_on, LOW);
|
|
Serial.print("Soy el robot ");
|
|
Serial.println(mac);
|
|
delay(1000);
|
|
HMC6352.Wake();
|
|
angulo_act = HMC6352.GetHeading();
|
|
HMC6352.Sleep();
|
|
angulo=0;
|
|
time_correc=millis();
|
|
pitidoPositivo();
|
|
}
|
|
|
|
void loop() //run over and over again
|
|
{
|
|
if (flag_giro==1) correccion_ang(); //Giro del robot
|
|
// SoftwareServo::refresh();
|
|
|
|
if ((analogRead(sensor1)<=19)&&(flag_avanza_stop==1))//Para ULTRASONIDO
|
|
{
|
|
flag_avanza_stop=0;
|
|
stop_motores();
|
|
pitidoParada();
|
|
}
|
|
else if (analogRead(sensor1)>50) flag_avanza_stop=1;
|
|
|
|
|
|
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)&&(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)&&(count==2))
|
|
{
|
|
dat = (val&0x0F)<<4;
|
|
count=3;
|
|
}
|
|
else if (((val>>4)==id)&&(count==3))
|
|
{
|
|
dat = dat|(val&0x0F);
|
|
count=4;
|
|
//Serial.print(0xF3, BYTE);
|
|
}
|
|
break;
|
|
case 0xF2:
|
|
if (((val>>4)==mac)&&(count==1))
|
|
{
|
|
id=val&0x0F;
|
|
count=0;
|
|
//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);
|
|
else if (dat==0x00) digital_shift(camara_on, LOW);
|
|
}
|
|
break;
|
|
case 0x04://Calibracion Brujula
|
|
{
|
|
if (dat==0x01)
|
|
{
|
|
//HMC6352.startcal() ;
|
|
giro_der(200,200);
|
|
}
|
|
else if (dat==0x00)
|
|
{
|
|
//HMC6352.stopcal() ;
|
|
stop_motores();
|
|
}
|
|
}
|
|
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();
|
|
|
|
while(digitalRead(CTS));
|
|
Serial.write(0xF1);
|
|
while(digitalRead(CTS));
|
|
Serial.write(id<<4|0x00);
|
|
while(digitalRead(CTS));
|
|
Serial.write(id<<4|((int (angulo_act/2))>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write(id<<4|((int (angulo_act/2))&0x0F));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id<<4)|(n_bat>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id<<4)|(n_bat&0x0F));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id<<4)|(IR1>>4));
|
|
while(digitalRead(CTS));
|
|
Serial.write((id<<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));
|
|
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;
|
|
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);
|
|
}
|
|
}
|