137 lines
2.9 KiB
C++
137 lines
2.9 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;
|
|
uint8_t timerTag;
|
|
|
|
unsigned long previousMillis = 0;
|
|
const long interval = 1000;
|
|
|
|
unsigned long timerMillis = 0;
|
|
long randomInterval = 1000;
|
|
|
|
|
|
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(2, 0, 0);
|
|
//Dynamixel.setCMargin(2, 1, 1);
|
|
|
|
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];
|
|
|
|
unsigned long currentMillis = millis();
|
|
if (currentMillis - previousMillis >= interval)
|
|
{
|
|
Dynamixel.torqueStatus(2,0);
|
|
Dynamixel.torqueStatus(1,0);
|
|
delay(30);
|
|
}
|
|
|
|
// unsigned long currentMillis2 = millis();
|
|
// if (currentMillis2 - timerMillis >= randomInterval)
|
|
// {
|
|
// timerMillis = millis();
|
|
// randomInterval = random(200, 2000);
|
|
// Dynamixel.torqueStatus(1,1);
|
|
// //Dynamixel.moveSpeed(2, 760, 1000); // OJOS OLD
|
|
// //Dynamixel.moveSpeed(1, 550, 1000); // BOCA OLD
|
|
// Dynamixel.moveSpeed(1, 100, 1000); // BOCA OLD
|
|
// delay(1000);
|
|
// //Dynamixel.moveSpeed(1, 720, 1000); // BOCA OLD
|
|
// Dynamixel.moveSpeed(1, 420, 1000); // BOCA OLD
|
|
// delay(1000);
|
|
// delay(1);
|
|
// Serial.println("random");
|
|
// delay(30);
|
|
//
|
|
// }
|
|
|
|
|
|
if(Zvalue != Zvalue_old)
|
|
{
|
|
Zvalue_old = Zvalue;
|
|
previousMillis = millis();
|
|
if(Zvalue)
|
|
{
|
|
Serial.println("Z->ON");
|
|
Dynamixel.torqueStatus(2,1);
|
|
Dynamixel.moveSpeed(2, 760, 1000); // OJOS
|
|
delay(1);
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Z->OFF");
|
|
Dynamixel.torqueStatus(2,1);
|
|
Dynamixel.moveSpeed(2, 300, 1000);
|
|
delay(1);
|
|
}
|
|
}
|
|
|
|
if(Cvalue != Cvalue_old)
|
|
{
|
|
Cvalue_old = Cvalue;
|
|
previousMillis = millis();
|
|
if(Cvalue)
|
|
{
|
|
Serial.println("C->ON");
|
|
Dynamixel.torqueStatus(1,1);
|
|
Dynamixel.moveSpeed(1, 550, 1000); // BOCA
|
|
delay(1);
|
|
}
|
|
else
|
|
{
|
|
Serial.println("C->OFF");
|
|
Dynamixel.torqueStatus(1,1);
|
|
Dynamixel.moveSpeed(1, 720, 1000);
|
|
delay(1);
|
|
}
|
|
}
|
|
|
|
}
|