Scribd
Upload
273 views8 pages

Arduino Code Self-Balancing Robot

This document contains code for controlling a robot using an MPU-6050 gyroscope/accelerometer and PID control. It initializes the MPU-6050, sets up PID control to balance the robot, and uses ultrasonic sensing to control punching motors based on distance to target. The main loop runs combo() which calls movement() to balance the robot and ultrasonic functions to control punching.

Uploaded by

Kevin Simons
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
273 views8 pages

Arduino Code Self-Balancing Robot

This document contains code for controlling a robot using an MPU-6050 gyroscope/accelerometer and PID control. It initializes the MPU-6050, sets up PID control to balance the robot, and uses ultrasonic sensing to control punching motors based on distance to target. The main loop runs combo() which calls movement() to balance the robot and ultrasonic functions to control punching.

Uploaded by

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

//youtube.

com/TARUNKUMARDAHAKE

//facebook.com/TARUNKUMARDAHAKE

#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

#include <NewPing.h>

NewPing sonar(A2 ,A3,10);

//enable pin

int enleft = A0;

int enright = A1;

//Motor pin

int rightpunchH = 3;

int rightpunchL =4;

int leftpunchH =12;

int leftpunchL =11;

int distance=0;

#define MIN_ABS_SPEED 20
MPU6050 mpu;

// 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 = 173;

double setpoint = originalSetpoint;

double movingAngleOffset = 0.1;

double input, output;

//adjust these values to fit your own design

double Kp = 76;

double Kd = 5 ;

double Ki = 1.5;

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

double motorSpeedFactorLeft = 0.6;

double motorSpeedFactorRight = 0.6;


//MOTOR CONTROLLER

int ENA = 5;

int IN1 = 6;

int IN2 = 7;

int IN3 = 8;

int IN4 = 9;

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

void dmpDataReady()

mpuInterrupt = true;

void setup()

// 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

Serial.begin(9600);

pinMode(enleft, OUTPUT);

pinMode(enright, OUTPUT);
pinMode(rightpunchH, OUTPUT);

pinMode(rightpunchL, OUTPUT);

pinMode(leftpunchH, OUTPUT);

pinMode(leftpunchH, OUTPUT);

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 nopunch()

{analogWrite(enright, 0);

analogWrite(enleft, 0);

digitalWrite(rightpunchH,0);

digitalWrite(rightpunchL,0);

digitalWrite(leftpunchH,0);

digitalWrite(leftpunchL,0);

}
void combo()

movement();

distance = sonar.ping_cm();

if (distance<=20)

{ analogWrite(enright, 80);

analogWrite(enleft, 100);

digitalWrite(rightpunchH,1);

digitalWrite(rightpunchL,0);

digitalWrite(leftpunchH,1);

digitalWrite(leftpunchL,0);

if (distance<=10)

{ analogWrite(enright, 100);

analogWrite(enleft, 130);

digitalWrite(rightpunchH,1);

digitalWrite(rightpunchL,0);

digitalWrite(leftpunchH,1);

digitalWrite(leftpunchL,0);

else

nopunch();

void movement()

// 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;

void loop()

{ combo();

You might also like

pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy