110 lines
2 KiB
C++
110 lines
2 KiB
C++
|
|
|
|
#include "DynamixelSerial.h"
|
|
#include <SPI.h>
|
|
#include "wiring_private.h"
|
|
|
|
#include <WiiChuck.h>
|
|
Accessory nunchuck1;
|
|
|
|
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 Ztag, Ctag;
|
|
|
|
void resetBoard()
|
|
{
|
|
//Serial.println("reset");
|
|
NVIC_SystemReset(); // esta funcion en teoria si funciona en SAMD
|
|
}
|
|
|
|
|
|
|
|
void setup() {
|
|
|
|
//Dynamixel.setID(1,2); // Solo para programar las IDs
|
|
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(512000);
|
|
//delay(4000);
|
|
//while (!Serial) {
|
|
//; // wait for serial port to connect. Needed for Leonardo only
|
|
//}
|
|
|
|
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() {
|
|
|
|
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(Zvalue != Zvalue_old)
|
|
{
|
|
Zvalue_old = Zvalue;
|
|
if(Zvalue)
|
|
{
|
|
Serial.println("Z->ON");
|
|
if(Ztag)
|
|
{
|
|
Dynamixel.moveSpeed(2, 1023, 512);
|
|
Ztag=0;
|
|
}
|
|
else
|
|
{
|
|
Dynamixel.moveSpeed(2, 0, 512);
|
|
Ztag=1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(Cvalue != Cvalue_old)
|
|
{
|
|
Cvalue_old = Cvalue;
|
|
if(Cvalue)
|
|
{
|
|
Serial.println("C->ON");
|
|
if(Ctag)
|
|
{
|
|
Dynamixel.moveSpeed(1, 1023, 1023);
|
|
Ctag=0;
|
|
}
|
|
else
|
|
{
|
|
Dynamixel.moveSpeed(1, 0, 1023);
|
|
Ctag=1;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|