quiero introducir sensores TRCT5000 al siguiente codigo pero no sé como;
[#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 30
MPU6050 mpu;
// MPU control/status vars//Control de MPU / variables de estado
bool dmpReady = false; // set true if DMP init was successful //Establecer verdadero si DMP init fue exitoso
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU//contiene el byte de estado de interrupción real de MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)//devolver el estado después de cada operación del dispositivo (0 = éxito,! 0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)// tamaño de paquete DMP esperado (el valor predeterminado es 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO//recuento de todos los bytes actualmente en FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer//FIFO buffer de almacenamiento
// orientation/motion vars//orientación / movimiento vars
Quaternion q; // [w, x, y, z] quaternion container// [w, x, y, z] contenedor de cuaternión
VectorFloat gravity; // [x, y, z] gravity vector// [x, y, z] vector de gravedad
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector// [yaw, pitch, roll] yaw / pitch / roll contenedor y vector de gravedad
//PID
double originalSetpoint = 173.0;//173.0
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
//adjust these values to fit your own design// ajusta estos valores para que se adapten a tu propio diseño
double Kp = 60;
double Kd = 2.2;
double Ki = 270;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
double motorSpeedFactorLeft = 0.3;
double motorSpeedFactorRight = 0.3;
//MOTOR CONTROLLER//CONTROLADOR DEL MOTOR
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 9;
int IN4 = 8;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high// indica si el pin de interrupción de MPU se ha elevado
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)// unirse al bus I2C (la biblioteca I2Cdev no hace esto automáticamente)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)// Reloj I2C de 400 kHz (200 kHz si la CPU es de 8 MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity// suministra aquí tus propios desplazamientos de giroscopio, escalados para una sensibilidad mínima
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip// 1688 predeterminado de fábrica para mi chip de prueba
// make sure it worked (returns 0 if so)// asegúrese de que funcionó (devuelve 0 si es así)
if (devStatus == 0)
{
// turn on the DMP, now that it's ready// Enciende la DMP, ahora que está lista
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection// habilita la detección de interrupciones Arduino
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it// habilite la detección de interrupciones de Arduino // establezca nuestro indicador DMP Ready para que la función main loop () sepa que está bien usarlo
dmpReady = true;
// get expected DMP packet size for later comparison// obtenga el tamaño de paquete DMP esperado para una comparación posterior
packetSize = mpu.dmpGetFIFOPacketSize();
//setup PID// PID de configuración
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else
{
// ERROR!
// 1 = initial memory load failed// 1 = error de carga de memoria inicial
// 2 = DMP configuration updates failed// 2 = Falló la actualización de la configuración de DMP
// (if it's going to break, usually the code will be 1)// (si se va a romper, generalmente el código será 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
// if programming failed, don't try to do anything// si la programación falló, no intentes hacer nada
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available// espere la interrupción de MPU o paquetes adicionales disponibles
while (!mpuInterrupt && fifoCount < packetSize)
{
//no mpu data - performing PID calculations and output to motors// sin datos de mpu - realizando cálculos PID y salida a motores
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
}
// reset interrupt flag and get INT_STATUS byte// restablecer el indicador de interrupción y obtener el byte INT_STATUS
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count// obtener el conteo FIFO actual
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)// verifica el desbordamiento (esto nunca debería suceder a menos que nuestro código sea demasiado ineficiente)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// reset so we can continue cleanly// restablecer para que podamos continuar limpiamente
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)// de lo contrario, verifique la interrupción de datos listos para DMP (esto debería ocurrir con frecuencia)
}
else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait// espera la longitud correcta de datos disponibles, debe ser una espera MUY corta
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO// lee un paquete de FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available// rastrea el recuento FIFO aquí en caso de que haya> 1 paquete disponible
// (this lets us immediately read more without waiting for an interrupt)// (esto nos permite leer más inmediatamente sin esperar una interrupción)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}]