292 lines
6.7 KiB
C++
292 lines
6.7 KiB
C++
|
|
#include <SPI.h>
|
|
//#include "DynamixelSerial.h"
|
|
//#include "wiring_private.h"
|
|
|
|
#include <WiiChuck.h>
|
|
Accessory nunchuck1;
|
|
|
|
#define ETHERNET_ON true
|
|
|
|
#include <Ethernet.h>
|
|
#include <EthernetUdp.h>
|
|
#include <OSCBundle.h>
|
|
#include <OSCBoards.h>
|
|
#include <OSCMessage.h>
|
|
|
|
// Enter a MAC address and IP address for your controller below.
|
|
// The IP address will be dependent on your local network:
|
|
byte mac[] = {
|
|
0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0x01
|
|
};
|
|
IPAddress ip(192, 168, 133, 2); // IP FEATHER1
|
|
IPAddress outIp(192, 168, 133, 4); // PLAYER1
|
|
//IPAddress ip(10, 100, 50, 3);
|
|
//IPAddress destIp(10, 100, 50, 2);
|
|
|
|
unsigned int localPort = 8888; // local port to listen on
|
|
unsigned int destPort = 2345; // TO SET SENDING PORT PLAYER
|
|
|
|
// An EthernetUDP instance to let us send and receive packets over UDP
|
|
EthernetUDP Udp;
|
|
OSCErrorCode error;
|
|
|
|
#define UDP_RX_PACKET_MAX_SIZE 8
|
|
|
|
uint32_t CMD[ 8 ]; // CMD + Ch + 2
|
|
|
|
uint8_t rele1 = A1;
|
|
uint8_t rele2 = A0;
|
|
|
|
uint8_t Zvalue;
|
|
uint8_t Cvalue;
|
|
uint8_t Zvalue_old = 0;
|
|
uint8_t Cvalue_old = 0;
|
|
|
|
uint8_t contador = 0;
|
|
unsigned long previousMillis = 0; // will store last time LED was updated
|
|
const long interval = 120000; //cambio cada 2min
|
|
|
|
void resetBoard()
|
|
{
|
|
//Serial.println("reset");
|
|
NVIC_SystemReset(); // esta funcion en teoria si funciona en SAMD
|
|
}
|
|
|
|
|
|
void test(OSCMessage &msg)
|
|
{
|
|
digitalWrite(13, HIGH);
|
|
Serial.println("test");
|
|
digitalWrite(13, LOW);
|
|
}
|
|
|
|
void reboot(OSCMessage &msg)
|
|
{
|
|
digitalWrite(13, HIGH);
|
|
Serial.println("reset");
|
|
resetBoard();
|
|
digitalWrite(13, LOW);
|
|
}
|
|
|
|
|
|
void cmdProcess( OSCMessage &msg )
|
|
{
|
|
unsigned int frequency = 0;
|
|
digitalWrite(13, HIGH);
|
|
//Serial.println("CMD received");
|
|
|
|
if (msg.isInt(0)){
|
|
CMD[0] = msg.getInt(0);
|
|
} //otherwise it's a floating point frequency in Hz
|
|
if (msg.isInt(1)){
|
|
CMD[1] = msg.getInt(1);
|
|
} //otherwise it's a floating point frequency in Hz
|
|
|
|
Serial.print(CMD[0]);
|
|
Serial.print(",");
|
|
Serial.print(CMD[1]);
|
|
Serial.println();
|
|
digitalWrite(13, LOW);
|
|
|
|
}
|
|
|
|
|
|
void scene( OSCMessage &msg )
|
|
{
|
|
digitalWrite(13, HIGH);
|
|
Serial.println("Scene received");
|
|
|
|
if (msg.isInt(0)){
|
|
CMD[0] = msg.getInt(0);
|
|
|
|
OSCMessage msg("/scene/change");
|
|
msg.add((int32_t)CMD[0]);
|
|
Udp.beginPacket(outIp, destPort);
|
|
msg.send(Udp);
|
|
Udp.endPacket();
|
|
msg.empty();
|
|
|
|
} //otherwise it's a floating point frequency in Hz
|
|
|
|
Serial.println(CMD[0]);
|
|
|
|
}
|
|
|
|
|
|
|
|
void setup() {
|
|
|
|
//Dynamixel.begin(1000000,5); // Inicialize the servo at 1Mbps and Pin Control 5
|
|
//Dynamixel.setCSlope(254, 64, 64);
|
|
//Dynamixel.setCMargin(254, 3, 3);
|
|
|
|
Serial.begin(115200);
|
|
//delay(4000);
|
|
//while (!Serial) {
|
|
//; // wait for serial port to connect. Needed for Leonardo only
|
|
//}
|
|
|
|
// start the Ethernet and UDP:
|
|
Ethernet.begin(mac, ip);
|
|
|
|
// Check for Ethernet hardware present
|
|
if (Ethernet.hardwareStatus() == EthernetNoHardware) {
|
|
Serial.println("Ethernet shield was not found. Sorry, can't run without hardware. :(");
|
|
while (true) {
|
|
delay(1); // do nothing, no point running without Ethernet hardware
|
|
}
|
|
}
|
|
if (Ethernet.linkStatus() == LinkOFF) {
|
|
Serial.println("Ethernet cable is not connected.");
|
|
}
|
|
|
|
Udp.begin(localPort);
|
|
|
|
//pinMode(rele1, OUTPUT);
|
|
//pinMode(rele2, OUTPUT);
|
|
|
|
nunchuck1.begin();
|
|
if (nunchuck1.type == Unknown_) { // Modificado en la libreria para que no de error con la Ethernet.h
|
|
nunchuck1.type = NUNCHUCK;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
void loop() {
|
|
|
|
OSCMessage msg;
|
|
int size;
|
|
|
|
unsigned long currentMillis = millis();
|
|
if (currentMillis - previousMillis >= interval)
|
|
{
|
|
// save the last time you blinked the LED
|
|
previousMillis = currentMillis;
|
|
contador++;
|
|
if(contador > 5)
|
|
contador = 1;
|
|
|
|
Serial.print("scena: ");
|
|
Serial.println(contador-1);
|
|
|
|
OSCMessage msg("/scene/change");
|
|
msg.add((int32_t)contador-1);
|
|
Udp.beginPacket(outIp, destPort);
|
|
msg.send(Udp);
|
|
Udp.endPacket();
|
|
msg.empty();
|
|
}
|
|
|
|
if ( (size = Udp.parsePacket()) > 0)
|
|
{
|
|
//Serial.print("mensaje recibido: ");
|
|
//Serial.println(size);
|
|
while (size--)
|
|
{
|
|
// msg.fill(Udp.read());
|
|
uint8_t packetBuffer[UDP_RX_PACKET_MAX_SIZE];
|
|
Udp.read(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
|
|
msg.fill(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
|
|
}
|
|
|
|
if (!msg.hasError())
|
|
{
|
|
//Serial.println(msg.match("/test"));
|
|
|
|
msg.dispatch("/test", test ); // this is how it is€ marked on the silkscreen
|
|
msg.dispatch("/reset", reboot ); // this is how it is€ marked on the silkscreen
|
|
msg.dispatch("/cmd", cmdProcess ); // this is how it is marked on the silkscreen
|
|
msg.dispatch("/scene", scene ); // this is how it is marked on the silkscreen
|
|
|
|
}
|
|
else
|
|
{
|
|
error = msg.getError();
|
|
//Serial.print("error: ");
|
|
//Serial.println(error);
|
|
}
|
|
}
|
|
|
|
nunchuck1.readData(); // Read inputs and update maps
|
|
//uint8_t joystickValueX = nunchuck1.values[0];
|
|
//uint8_t joystickValueY = nunchuck1.values[1];
|
|
Zvalue = nunchuck1.values[10];
|
|
Cvalue = nunchuck1.values[11];
|
|
|
|
// Serial.print("X: ");
|
|
// Serial.print(joystickValueX);
|
|
// Serial.print(" Y: ");
|
|
// Serial.print(joystickValueY);
|
|
//Serial.print(" Z: ");
|
|
//Serial.print(Zvalue);
|
|
//Serial.print(" C: ");
|
|
//Serial.println(Cvalue);
|
|
|
|
if(Cvalue != Cvalue_old)
|
|
{
|
|
Cvalue_old = Cvalue;
|
|
if(Cvalue)
|
|
{
|
|
Serial.println("C->ON");
|
|
contador++;
|
|
if(contador > 5)
|
|
contador = 1;
|
|
|
|
Serial.print("scena: ");
|
|
Serial.println(contador-1);
|
|
|
|
OSCMessage msg("/scene/change");
|
|
msg.add((int32_t)contador-1);
|
|
Udp.beginPacket(outIp, destPort);
|
|
msg.send(Udp);
|
|
Udp.endPacket();
|
|
msg.empty();
|
|
}
|
|
}
|
|
|
|
if(Zvalue != Zvalue_old)
|
|
{
|
|
Zvalue_old = Zvalue;
|
|
if(Zvalue)
|
|
{
|
|
Serial.println("Z->ON");
|
|
contador--;
|
|
if(contador < 1)
|
|
contador = 5;
|
|
|
|
Serial.print("scena: ");
|
|
Serial.println(contador-1);
|
|
|
|
OSCMessage msg("/scene/change");
|
|
msg.add((int32_t)contador-1);
|
|
Udp.beginPacket(outIp, destPort);
|
|
msg.send(Udp);
|
|
Udp.endPacket();
|
|
msg.empty();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
Dynamixel.move(1,random(200,800)); // Move the Servo radomly from 200 to 800
|
|
delay(1);
|
|
//Dynamixel.move(1,joystickValueX*4); // Move the Servo radomly from 200 to 800
|
|
|
|
Serial.print("Posicion: ");
|
|
Serial.println(Dynamixel.readPosition(1));
|
|
//digitalWrite(rele1, HIGH);
|
|
//digitalWrite(rele2, HIGH);
|
|
//delay(500);
|
|
//digitalWrite(rele1, LOW);
|
|
//digitalWrite(rele2, LOW);
|
|
delay(500);
|
|
*/
|
|
|
|
|
|
}
|