476 lines
15 KiB
C++
476 lines
15 KiB
C++
#include <DmxSimple.h>
|
|
const int ledPin = 13; // Numero del pin para el Led
|
|
const int EnTxPin = 49; // HIGH:TX y LOW:RX
|
|
#define rs485 Serial2
|
|
#define DMX_ON false
|
|
char c;
|
|
const int id[8] = {101, 102, 103, 104, 105, 106, 107, 108};
|
|
int limit[8] = {360, 360, 360, 360, 360, 360, 360, 360};
|
|
int center[8] = {180, 180, 180, 180, 180, 180, 180, 180};
|
|
String inputString;
|
|
|
|
const int foco[4] = {313, 305, 226, 297};
|
|
const int strobe[4] = {247, 61, 69, 233};
|
|
|
|
unsigned long time_strobe = millis();
|
|
unsigned long time_foco = millis();
|
|
unsigned long time_mov = millis();
|
|
unsigned long time_stop = millis();
|
|
unsigned long time_escape = millis();
|
|
int cont_foco = 0;
|
|
int cont_mov = 0;
|
|
byte port = 0;
|
|
|
|
byte mode = 0;
|
|
bool flag_mode0 = false;
|
|
|
|
int CMD[ 64 ]; // CMD + Ch + 6
|
|
bool flag_ramp = false;
|
|
byte inc = 0;
|
|
boolean flag_center= false;
|
|
|
|
void FC1_CHANGE() { //PD3
|
|
port = PIND & 0x08; //B00001100;
|
|
if (port == 0x00) mode = 1;
|
|
else mode = 0;
|
|
Serial.println(mode);
|
|
}
|
|
|
|
void FC2_CHANGE() { //PD2
|
|
port = PIND & 0x04; //B00001100;
|
|
if (port == 0x00) mode=2;
|
|
else mode = 0;
|
|
Serial.println(mode);
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
rs485.begin(115200);
|
|
rs485.setTimeout(100); //establecemos un tiempo de espera de 100ms
|
|
// inicializamos los pines
|
|
pinMode(ledPin, OUTPUT);
|
|
pinMode(EnTxPin, OUTPUT);
|
|
pinMode(18, INPUT);
|
|
pinMode(19, INPUT);
|
|
digitalWrite(ledPin, LOW);
|
|
attachInterrupt(5, FC1_CHANGE, CHANGE);//PIN18 PD3 FC1
|
|
attachInterrupt(4, FC2_CHANGE, CHANGE);//PIN19 PD2 FC2
|
|
|
|
/* The most common pin for DMX output is pin 3, which DmxSimple
|
|
** uses by default. If you need to change that, do it here. */
|
|
DmxSimple.usePin(15);
|
|
|
|
/* DMX devices typically need to receive a complete set of channels
|
|
** even if you only need to adjust the first channel. You can
|
|
** easily change the number of channels sent here. If you don't
|
|
** do this, DmxSimple will set the maximum channel number to the
|
|
** highest channel you DmxSimple.write() to. */
|
|
DmxSimple.maxChannel(512);
|
|
strobe_control(0);
|
|
time_strobe = millis();
|
|
time_foco = millis();
|
|
time_stop = millis();
|
|
cont_foco = 0;
|
|
cont_mov=0;
|
|
if (digitalRead(18)&&digitalRead(19)) mode=0;
|
|
else if((!digitalRead(18))&&digitalRead(19)) mode=1;
|
|
else if(digitalRead(18)&&(!digitalRead(19))) mode=2;
|
|
}
|
|
|
|
void strobe_control(int num, int val1, int val2)
|
|
{
|
|
DmxSimple.write(strobe[num], 255);
|
|
DmxSimple.write(strobe[num]+4, 255);
|
|
DmxSimple.write(strobe[num]+5, val1);
|
|
DmxSimple.write(strobe[num]+6, val2);
|
|
// for(int i=0; i<8; i++)
|
|
// {
|
|
// DmxSimple.write(strobe[0]+i, val);
|
|
// DmxSimple.write(strobe[1]+i, val);
|
|
// DmxSimple.write(strobe[2]+i, val);
|
|
// DmxSimple.write(strobe[3]+i, val);
|
|
// }
|
|
|
|
}
|
|
|
|
void strobe_control(int val)
|
|
{
|
|
if (val==0)
|
|
{
|
|
for(int i=0; i<8; i++)
|
|
{
|
|
DmxSimple.write(strobe[0]+i, val);
|
|
DmxSimple.write(strobe[1]+i, val);
|
|
DmxSimple.write(strobe[2]+i, val);
|
|
DmxSimple.write(strobe[3]+i, val);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(int i=0; i<4; i++)
|
|
{
|
|
strobe_control(i, 128, val);
|
|
}
|
|
}
|
|
|
|
}
|
|
void foco_control(int num, int red, int green, int blue, int white)
|
|
{
|
|
DmxSimple.write(foco[num], red);
|
|
DmxSimple.write(foco[num]+1, green);
|
|
DmxSimple.write(foco[num]+2, blue);
|
|
DmxSimple.write(foco[num]+3, white);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
if (mode==0)
|
|
{
|
|
flag_center=false;
|
|
if (!flag_mode0)
|
|
{
|
|
strobe_control(0);
|
|
foco_control(0, 0, 0, 0, 0);
|
|
foco_control(1, 0, 0, 0, 0);
|
|
foco_control(2, 0, 0, 0, 0);
|
|
foco_control(3, 0, 0, 0, 0);
|
|
flag_mode0=true;
|
|
}
|
|
|
|
if (Serial.available())
|
|
{
|
|
c = Serial.read();
|
|
//Serial.print(c);
|
|
|
|
if ((c == '\r') || (c == '\n'))
|
|
{
|
|
if (inputString == "ping")
|
|
{
|
|
Serial.println("received ping to all");
|
|
for (int i=0; i<8; i++)
|
|
{
|
|
time_escape=millis();
|
|
while ((ping(id[i])==-1)&&(millis()-time_escape<500));
|
|
//delay(1);
|
|
}
|
|
}
|
|
else if (inputString.startsWith("ping "))
|
|
{
|
|
String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
|
|
Serial.print("received ping to ");
|
|
int direction = str.toInt();
|
|
Serial.println(direction);
|
|
ping(id[direction-1]);
|
|
}
|
|
if (inputString == "lim")
|
|
{
|
|
Serial.println("received limit to all");
|
|
for (int i=0; i<8; i++)
|
|
{
|
|
limit[i] = lim(id[i]);
|
|
time_escape=millis();
|
|
while ((limit[i]==-1)&&(millis()-time_escape<500))
|
|
{
|
|
limit[i] = lim(id[i]);
|
|
}
|
|
//delay(1);
|
|
}
|
|
}
|
|
else if (inputString.startsWith("lim "))
|
|
{
|
|
String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
|
|
Serial.print("received limit to ");
|
|
int direction = str.toInt();
|
|
Serial.println(direction);
|
|
limit[direction-1] = lim(id[direction-1]);
|
|
while (limit[direction-1]==-1)
|
|
{
|
|
limit[direction-1] = lim(id[direction-1]);
|
|
}
|
|
}
|
|
else if (inputString == "center")
|
|
{
|
|
Serial.println("Center to all");
|
|
for (int i=0; i<8; i++)
|
|
{
|
|
sendData(id[i], 0, 100);
|
|
delay(100);
|
|
}
|
|
}
|
|
else if (inputString.startsWith("center "))
|
|
{
|
|
String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
|
|
Serial.print("center to ");
|
|
int direction = str.toInt();
|
|
Serial.println(direction-1);
|
|
sendData(id[direction-1], limit[direction-1]/2, 100);
|
|
}
|
|
else if (inputString == "cal")
|
|
{
|
|
Serial.println("calibrating all");
|
|
calibrationData(101);
|
|
calibrationData(102);
|
|
calibrationData(103);
|
|
calibrationData(104);
|
|
calibrationData(105);
|
|
calibrationData(106);
|
|
calibrationData(107);
|
|
calibrationData(108);
|
|
}
|
|
else if (inputString.startsWith("cal "))
|
|
{
|
|
String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
|
|
Serial.print("calibrating ");
|
|
int direction = str.toInt();
|
|
Serial.println(direction);
|
|
calibrationData(id[direction-1]);
|
|
}
|
|
else if (inputString.startsWith("send:"))
|
|
{
|
|
Serial.print("send: ");
|
|
String str = inputString.substring(inputString.lastIndexOf(":") + 1);
|
|
int direction = str.toInt();
|
|
|
|
String command = str.substring(0, str.indexOf(','));
|
|
command.trim();
|
|
|
|
int index = 0;
|
|
int commaIndex = 0;
|
|
do {
|
|
commaIndex = str.indexOf(',', commaIndex + 1);
|
|
String tmp = str.substring(commaIndex + 1, str.indexOf(',', commaIndex + 1));
|
|
tmp.trim();
|
|
if (isNumeric(tmp)) {
|
|
CMD[index] = tmp.toInt();
|
|
index++;
|
|
}
|
|
} while (str.lastIndexOf(',') != commaIndex);
|
|
int angle = CMD[0];
|
|
int speed = CMD[1];
|
|
Serial.print(direction);
|
|
Serial.print(", ");
|
|
Serial.print(angle);
|
|
Serial.print(", ");
|
|
Serial.println(speed);
|
|
sendData(id[direction-1], angle, speed);
|
|
}
|
|
inputString = String();
|
|
|
|
}
|
|
else
|
|
inputString += c;
|
|
}
|
|
}
|
|
else if (mode==1)
|
|
{
|
|
flag_mode0=false;
|
|
if(!flag_center)
|
|
{
|
|
strobe_control(255);
|
|
foco_control(0, 0, 0, 0, 255);
|
|
foco_control(1, 0, 0, 0, 255);
|
|
foco_control(2, 0, 0, 0, 255);
|
|
foco_control(3, 0, 0, 0, 255);
|
|
for (int i=0; i<=7; i++) sendData(id[i], 0, 100);
|
|
delay(10000);
|
|
center_all(70000);
|
|
flag_center=true;
|
|
strobe_control(0);
|
|
}
|
|
else
|
|
{
|
|
if (flag_ramp)
|
|
{
|
|
foco_control(0, 0, 0, 0, inc);
|
|
foco_control(1, 0, 0, 0, inc);
|
|
foco_control(2, 0, 0, 0, inc);
|
|
foco_control(3, 0, 0, 0, inc);
|
|
inc++;
|
|
if(inc==255) flag_ramp=false;
|
|
delay(10);
|
|
}
|
|
else
|
|
{
|
|
foco_control(0, 0, 0, 0, inc);
|
|
foco_control(1, 0, 0, 0, inc);
|
|
foco_control(2, 0, 0, 0, inc);
|
|
foco_control(3, 0, 0, 0, inc);
|
|
inc--;
|
|
if(inc==0) flag_ramp=true;
|
|
delay(10);
|
|
}
|
|
}
|
|
}
|
|
else if (mode==2)
|
|
{
|
|
flag_center=false;
|
|
flag_mode0=false;
|
|
if ((millis()-time_foco)>500)
|
|
{
|
|
if (cont_foco==0)foco_control(3, 0, 0, 0, 0);
|
|
else if(cont_foco>4) cont_foco=0;
|
|
else foco_control(cont_foco-1, 0, 0, 0, 0);
|
|
foco_control(cont_foco, 0, 0, 0, 255);
|
|
strobe_control(0);
|
|
time_foco=millis();
|
|
cont_foco++;
|
|
}
|
|
if ((millis()-time_mov)>20000)
|
|
{
|
|
if (cont_mov==2)
|
|
{
|
|
strobe_control(random(20,255));
|
|
foco_control(0, 0, 0, 0, 255);
|
|
foco_control(1, 0, 0, 0, 255);
|
|
foco_control(2, 0, 0, 0, 255);
|
|
foco_control(3, 0, 0, 0, 255);
|
|
delay(1000);
|
|
for (int i=0; i<=7; i++) sendData(id[i], 0, 100);
|
|
delay(2000);
|
|
center_all(70000);
|
|
}
|
|
else if(cont_mov>=10) cont_mov=-1;
|
|
else
|
|
{
|
|
for(int i=0; i<=7; i++)
|
|
{
|
|
sendData(id[i], random(0,360), 100);
|
|
delay(100);
|
|
}
|
|
time_mov=millis();
|
|
}
|
|
Serial.println(cont_mov);
|
|
cont_mov++;
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|
|
void center_all(unsigned long time)
|
|
{
|
|
// for (int i=0; i<=7; i++)
|
|
// {
|
|
// sendData(id[i], 0, 0);
|
|
// delay(1000);
|
|
// }
|
|
//sendData(id[i], 0, 0);
|
|
calibrationData(101);
|
|
calibrationData(102);
|
|
calibrationData(103);
|
|
calibrationData(104);
|
|
calibrationData(105);
|
|
calibrationData(106);
|
|
calibrationData(107);
|
|
calibrationData(108);
|
|
// for (int i=0; i<=7; i++)
|
|
// {
|
|
// calibrationData(id[i]);
|
|
// delay(1000);
|
|
// }
|
|
delay(time);
|
|
}
|
|
void sendData(int direction, int angle, int speed)
|
|
{
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
|
|
//---enviamos el ángulo para mover el servo------
|
|
rs485.print("I"); //inicio de trama
|
|
rs485.print(direction);//dirección del esclavo
|
|
rs485.print("S"); //función S para indicarle el angulo
|
|
rs485.print(angle); //ángulo o dato
|
|
rs485.print("V"); //función V para indicarle que velocidad tendra.
|
|
rs485.print(speed); //ángulo o dato
|
|
rs485.print("F"); //fin de trama
|
|
//----------------------------
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como receptor
|
|
}
|
|
|
|
bool state = false;
|
|
int ping(int direction) //Responde con el angulo actual;
|
|
{
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
|
|
//---solicitamos una lectura del sensor----------
|
|
rs485.print("I"); //inicio de trama
|
|
rs485.print(direction);//direccion del esclavo
|
|
rs485.print("L"); //L para indicarle que vamos a Leer el angulo actual
|
|
rs485.print("F"); //fin de trama
|
|
rs485.flush(); //Esperamos hasta que se envíen los datos
|
|
//----Leemos la respuesta del Esclavo-----
|
|
digitalWrite(EnTxPin, LOW); //RS485 como receptor
|
|
//delay(10);
|
|
|
|
time_escape = millis();
|
|
state = rs485.find("i");
|
|
while(!state&&((millis()-time_escape)<=500))state = rs485.find("i");
|
|
//while(rs485.find("i"))//&&((millis()-time_escape)<=500)) //esperamos el inicio de trama
|
|
if (state)
|
|
{
|
|
int esclavo=rs485.parseInt(); //recibimos la direccion del esclavo
|
|
int dato=rs485.parseInt(); //recibimos el dato
|
|
if(rs485.read()=='f'&&esclavo==direction) //si fin de trama y direccion son los correctos
|
|
{
|
|
Serial.print(direction);
|
|
Serial.print(" ");
|
|
Serial.println(dato);
|
|
return dato;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int lim(int direction) //Responde con el angulo actual;
|
|
{
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
|
|
//---solicitamos una lectura del sensor----------
|
|
rs485.print("I"); //inicio de trama
|
|
rs485.print(direction);//direccion del esclavo
|
|
rs485.print("A"); //L para indicarle que vamos a Leer el limite de angulo
|
|
rs485.print("F"); //fin de trama
|
|
rs485.flush(); //Esperamos hasta que se envíen los datos
|
|
//----Leemos la respuesta del Esclavo-----
|
|
digitalWrite(EnTxPin, LOW); //RS485 como receptor
|
|
//delay(1);
|
|
time_escape = millis();
|
|
state = rs485.find("i");
|
|
while(!state&&((millis()-time_escape)<=500))state = rs485.find("i");
|
|
//while(rs485.find("i"))//&&((millis()-time_escape)<=500)) //esperamos el inicio de trama
|
|
if (state)
|
|
{
|
|
int esclavo=rs485.parseInt(); //recibimos la direccion del esclavo
|
|
int dato=rs485.parseInt(); //recibimos el dato
|
|
if(rs485.read()=='f'&&esclavo==direction) //si fin de trama y direccion son los correctos
|
|
{
|
|
Serial.print(direction);
|
|
Serial.print(" ");
|
|
Serial.print(dato);
|
|
Serial.print(" ");
|
|
Serial.println(dato/2);
|
|
return dato;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
void calibrationData(int direction)
|
|
{
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
|
|
//---enviamos el ángulo para mover el servo------
|
|
rs485.print("I"); //inicio de trama
|
|
rs485.print(direction);//dirección del esclavo
|
|
rs485.print("C"); //función C para indicarle el comando de calibrado.
|
|
rs485.print("F"); //fin de trama
|
|
rs485.flush(); //Esperamos hasta que se envíen los datos
|
|
//----------------------------
|
|
digitalWrite(EnTxPin, HIGH); //RS485 como receptor
|
|
}
|
|
|
|
boolean isNumeric(String str) {
|
|
for (char i = 0; i < str.length(); i++) {
|
|
if ( !(isDigit(str.charAt(i)) )) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|