miguel/Lab_interaccio
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity
Lab_interaccio/2016/BOB_FAKE/BOB_FAKE.ino

622 lines
15 KiB
Arduino
Raw Normal View History

second commit
2025-02-25 21:29:42 +01:00
/*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 vertical;
//SoftwareServo horizontal;
Servo vertical;
Servo horizontal;
//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;
int val = 0x00; //Valor entrada serie
int n_bat = 0;
int Command_ok=0;
int mensaje_ok=0;
int modo=0;
int 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();
vertical.attach(2);
// vertical.writeMicroseconds(1500);
horizontal.attach(3);
// horizontal.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);
vertical.write(MED_VER); //Vertical
horizontal.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();
}
int distancia = 100;
void loop() //run over and over again
{
//if (flag_giro==1) correccion_ang(); //Giro del robot
// for (int i=MIN_VER; i<MAX_VER; i++)
// {
// vertical.write(i); //Vertical
// delay(15);
// }
// for (int i=MAX_VER; i>MIN_VER; i--)
// {
// vertical.write(i); //Vertical
// delay(15);
// }
for (int i=MIN_HOR; i<MAX_HOR; i++)
{
horizontal.write(i); //Vertical
delay(15);
digital_shift(camara_on, LOW);
while ((analogRead(sensor1)<=distancia)&&(flag_avanza_stop==1))
{
digital_shift(camara_on, HIGH);//Para ULTRASONIDO
}
}
for (int i=MAX_HOR; i>MIN_HOR; i--)
{
horizontal.write(i); //Vertical
delay(15);
while ((analogRead(sensor1)<=distancia)&&(flag_avanza_stop==1))
{
digital_shift(camara_on, HIGH);//Para ULTRASONIDO
}
}
// 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(dat*2,dat*2);
// 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);
// vertical.write(60);
// horizontal.write(90);
// delay(15);
// }
// break;
// case 0x0A: //Giro cuello vertical
// {
// digital_shift(servo_on, LOW);
// vertical.write(map(dat, 0, 180, 0, 114)+66);
// delay(15);
// }
// break;
// case 0x0B: //Giro cuello Horizontal
// {
// digital_shift(servo_on, LOW);
// if (dat>180) dat=180;
// horizontal.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);
}
}
Reference in a new issue Copy permalink