#include #include #define bit9600Delay 84 #define halfBit9600Delay 42 #define bit4800Delay 188 #define halfBit4800Delay 94 #undef int #undef abs #undef double #undef float #undef round #include "WProgram.h" void SWprint(int data); void izquierda(int iz); void derecha(int der); void setup(); void loop(); Servo servoA, servoB, servoC; int IR = 8; int zumbador =11; int camara = 4; int girocam = 2; int bat = 4; int bit1 = 14; int bit2 = 15; int bit3 = 16; int bit4 = 17; int led =3; int laser =5; int act_serv = 7; byte rx = 9; byte tx = 10; int id = 0; int val1 = 0; int val2 = 0; int val3 = 0; int val4 = 0; byte val = 0; unsigned long time=0; unsigned long time_ant=0; int frec = 0; int activa=0; int Command_ok=0; int mensaje_ok=0; int dat = 0; int datd = 0; int dati = 0; int command = 0; int count = 0; void SWprint(int data) { byte mask; //startbit digitalWrite(tx,LOW); delayMicroseconds(bit9600Delay); for (mask = 0x01; mask>0; mask <<= 1) { if (data & mask){ // choose bit digitalWrite(tx,HIGH); // send 1 } else{ digitalWrite(tx,LOW); // send 0 } delayMicroseconds(bit9600Delay); } //stop bit digitalWrite(tx, HIGH); delayMicroseconds(bit9600Delay); } void izquierda(int iz) { SWprint(toupper(1)); } void derecha(int der) { SWprint(toupper(2)); } void setup() { Serial.begin(19200); delay(100); /*Serial.print("ER_CMD#U5"); delay(200); Serial.print("ACK"); delay(200); Serial.begin(38400);*/ servoC.attach(girocam); pinMode(rx,INPUT); pinMode(tx,OUTPUT); digitalWrite(tx,HIGH); digitalWrite(act_serv, HIGH); //Servos apagados servoC.write(93); //Parado pinMode(IR, OUTPUT); pinMode(zumbador, OUTPUT); pinMode(camara, OUTPUT); pinMode(14, INPUT); pinMode(15, INPUT); pinMode(16, INPUT); pinMode(17, INPUT); digitalWrite(14, HIGH); digitalWrite(15, HIGH); digitalWrite(16, HIGH); digitalWrite(17, HIGH); analogWrite(zumbador,0); if (digitalRead(bit1)) val1=1; else val1=0; if (digitalRead(bit2)) val2=2; else val2=0; if (digitalRead(bit3)) val3=4; else val3=0; if (digitalRead(bit4)) val4=8; else val4=0; id=val1+val2+val3+val4; Serial.print("Hola, soy el robot numero "); Serial.println(id); digitalWrite(camara, LOW); //Desactiva camara digitalWrite(act_serv, HIGH); //Desactiva servomotores digitalWrite(led, LOW); //Apaga leds digitalWrite(laser, LOW); //Apaga laser //Serial.print(id<<4|0x00, BYTE); //Serial.print(id<<4|analogRead(bat)/68, BYTE); } void loop() { Servo::refresh(); if (Serial.available()) { val = Serial.read(); if (val==0xF0) { mensaje_ok=1; count=1; } else if (mensaje_ok==1) count++; if (count==2) { if ((val>>4)==id) command=val&0x0F; else { mensaje_ok=0; count=0; } } else if (count==3) { if ((val>>4)==id) dat = val&0x0F; else { mensaje_ok=0; count=0; } } //else if (count>3) Serial.print(0xF1, BYTE); // } if ((count==3)&&(mensaje_ok==1)) { count=0; mensaje_ok=0; //map(pos, 0,180,5,200); delay(10); switch (command) { case 0x00: //Servo derecho switch (dat) { case 0x00: datd=180; break; case 0x01: datd=100; break; case 0x02: datd=90; break; case 0x03: datd=80; break; case 0x04: datd=70; break; case 0x05: datd=60; // break; case 0x06: datd=50; break; case 0x07: datd=38; break; case 0x08: datd=38; break; case 0x09: datd=25; break; case 0x0A: datd=15; break; case 0x0B: datd=10; break; case 0x0C: datd=8; break; case 0x0D: datd=6; break; case 0x0E: datd=3; break; case 0x0F: datd=0; break; } //if (dat==7) digitalWrite(act_serv, HIGH); digitalWrite(act_serv, LOW); if (datd>=180) derecha(datd); break; case 0x01: //Servo izquierdo switch (dat) { case 0x00: dati=0; break; case 0x01: dati=3; break; case 0x02: dati=6; break; case 0x03: dati=8; break; case 0x04: dati=10; break; case 0x05: dati=15; break; case 0x06: dati=25; break; case 0x07: dati=38; break; case 0x08: dati=38; break; case 0x09: dati=50; break; case 0x0A: dati=60; break; case 0x0B: dati=70; break; case 0x0C: dati=80; break; case 0x0D: dati=90; break; case 0x0E: dati=100; break; case 0x0F: dati=180; break; } //if (dat==7) digitalWrite(act_serv, HIGH); digitalWrite(act_serv, LOW); if (dati>=180) izquierda(dati); break; case 0x02: //CAMARA ON/OFF { if (dat==0x01) digitalWrite(camara, HIGH); else digitalWrite(camara, LOW); } break; case 0x03: //GIRO CAMARA { digitalWrite(act_serv, LOW); servoC.write(11*dat); } break; case 0x04://LASER ON/OFF { if (dat==0x01) digitalWrite(laser, HIGH); else digitalWrite(laser, LOW); } break; case 0x05: //ZUMBADOR { if (dat==0x01) { frec= 100; activa=1; time_ant=millis(); } else { activa=0; analogWrite(zumbador,0); } } break; case 0x06: //IR { if (dat==0x01) digitalWrite(IR, HIGH); else digitalWrite(IR, LOW); } break; case 0x07: //LEDS BLANCOS { if (dat==0x01) digitalWrite(led, HIGH); //Enciende leds else digitalWrite(led, LOW); } break; case 0x08: // { //if (dat==0x01) digitalWrite(act_serv, HIGH); //Apaga servo } break; case 0x09: //comando off servos { digitalWrite(act_serv, HIGH); //STOP SERVO SWprint(toupper(4)); } break; } //Comando de vuelta //Serial.print(id<<4|command, BYTE); //Serial.print(id<<4|analogRead(bat)/68, BYTE); } time = millis(); if (activa==1) { if (time-time_ant >= frec) { analogWrite(zumbador,255); if (time-time_ant >= 2*frec) time_ant=time; } else analogWrite(zumbador,0); } } int main(void) { init(); setup(); for (;;) loop(); return 0; }