Scribd
Upload
136 views4 pages

Codigo Balancing

This document contains code for controlling a robot using an MPU-6050 gyroscope/accelerometer and PID control. It initializes the MPU-6050 and sets up PID control to maintain the robot's orientation. It then enters a loop that reads orientation data from the MPU, calculates the PID output to control two motors via PWM, and allows Bluetooth control to adjust the setpoint orientation.

Uploaded by

Pablo Alvarez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
136 views4 pages

Codigo Balancing

This document contains code for controlling a robot using an MPU-6050 gyroscope/accelerometer and PID control. It initializes the MPU-6050 and sets up PID control to maintain the robot's orientation. It then enters a loop that reads orientation data from the MPU, calculates the PID output to control two motors via PWM, and allows Bluetooth control to adjust the setpoint orientation.

Uploaded by

Pablo Alvarez
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

#include <PID_v1.

h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 30
MPU6050 mpu;
//*************************************** Ajustes
***************************************
double MotorVelocidadIzq = 0.5; //double MotorVelocidadIzq = 0.3;
double MotorVelocidadDer = 0.5; //double MotorVelocidadDer = 0.3;
double PuntoEquilibrio = 180.8;
SoftwareSerial Serial_2 (4, 3);//SoftwareSerial Serial_2 (1, 0);
//-----------------Control de Motores
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 9;
int IN4 = 8;
int ENB = 10;
//------------------Los Valors de PID cambian con cada dise�o
double Kp = 60; //double Kp = 60;
double Kd = 3.5; //double Kd = 2.2;
double Ki = 250; //double Ki = 270;

//*********************************************************************************
******
int estado = 'g'; // inicia detenido
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 =
error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

//PID

double originalSetpoint = PuntoEquilibrio; //double originalSetpoint = 172.50;

double setpoint = originalSetpoint;


double movingAngleOffset = 0.1;
double input, output;

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = MotorVelocidadIzq; //double motorSpeedFactorLeft =


0.6;
double motorSpeedFactorRight = MotorVelocidadDer; //double motorSpeedFactorRight =
0.5;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4,


motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone
high
void dmpDataReady()
{
mpuInterrupt = true;
}

void setup()
{
Serial_2.begin(9600); // inicia el puerto serial para comunicacion con el
Bluetooth
//Serial.begin(9600);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#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
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)


if (devStatus == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection


attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;

// get expected DMP packet size for later comparison


packetSize = mpu.dmpGetFIFOPacketSize();

//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}

void loop()
{

//********************************************** Control Blu


************************
if(Serial_2.available()>0){ // lee el bluetooth y almacena en estado
estado = Serial_2.read();
}
if(estado=='a'){ // Boton desplazar al Frente
setpoint = (setpoint + 0.5);
//Serial.println(originalSetpoint);
estado = 'g';
}
if(estado=='b'){ // Boton IZQ

estado = 'g';
}
if(estado=='c'){ // Boton Parar
setpoint = PuntoEquilibrio;

estado = 'g';
}
if(estado=='d'){ // Boton DER

estado = 'g';
}

if(estado=='e'){ // Boton Reversa


setpoint = (setpoint - 0.5);
estado = 'g';
}

//********************************************** Fin Control Blu


************************
// if programming failed, don't try to do anything
if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available


while (!mpuInterrupt && fifoCount < packetSize)
{
//no mpu data - performing PID calculations and output to motors
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);

// reset interrupt flag and get INT_STATUS byte


mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count


fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too
inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO


mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available


// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}

You might also like