Components and supplies
Dual H-Bridge motor drivers L293D
Jumper wires (generic)
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
3.7 V LiPo Battery
Arduino UNO
Apps and platforms
Arduino IDE
Project description
Code
arduino code
arduino
upload the sketch
1#include <PID_v1.h> 2#include <LMotorController.h> 3#include "I2Cdev.h" 4#include "MPU6050_6Axis_MotionApps20.h" 5 6#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 7 #include "Wire.h" 8#endif 9 10#define MIN_ABS_SPEED 20 11 12MPU6050 mpu; 13 14// MPU control/status vars 15bool dmpReady = false; // set true if DMP init was successful 16uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 17uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 18uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 19uint16_t fifoCount; // count of all bytes currently in FIFO 20uint8_t fifoBuffer[64]; // FIFO storage buffer 21 22// orientation/motion vars 23Quaternion q; // [w, x, y, z] quaternion container 24VectorFloat gravity; // [x, y, z] gravity vector 25float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 26 27//PID 28double originalSetpoint =167; 29double setpoint = originalSetpoint; 30double movingAngleOffset = 0.1; 31double input, output; 32 33//adjust these values to fit your own design 34double Kp = 100; 35double Kd = 8; 36double Ki = 60; 37PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT); 38 39double motorSpeedFactorLeft = 0.6; 40double motorSpeedFactorRight = 0.6; 41//MOTOR CONTROLLER 42int ENA = 5; 43int IN1 = 6; 44int IN2 = 7; 45int IN3 = 8; 46int IN4 = 9; 47int ENB = 10; 48LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight); 49 50volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 51void dmpDataReady() 52{ 53 mpuInterrupt = true; 54} 55 56 57void setup() 58{ 59 // join I2C bus (I2Cdev library doesn't do this automatically) 60 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 61 Wire.begin(); 62 TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 63 #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 64 Fastwire::setup(400, true); 65 #endif 66 67 mpu.initialize(); 68 69 devStatus = mpu.dmpInitialize(); 70 71 // supply your own gyro offsets here, scaled for min sensitivity 72 mpu.setXGyroOffset(220); 73 mpu.setYGyroOffset(76); 74 mpu.setZGyroOffset(-85); 75 mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 76 77 // make sure it worked (returns 0 if so) 78 if (devStatus == 0) 79 { 80 // turn on the DMP, now that it's ready 81 mpu.setDMPEnabled(true); 82 83 // enable Arduino interrupt detection 84 attachInterrupt(0, dmpDataReady, RISING); 85 mpuIntStatus = mpu.getIntStatus(); 86 87 // set our DMP Ready flag so the main loop() function knows it's okay to use it 88 dmpReady = true; 89 90 // get expected DMP packet size for later comparison 91 packetSize = mpu.dmpGetFIFOPacketSize(); 92 93 //setup PID 94 pid.SetMode(AUTOMATIC); 95 pid.SetSampleTime(10); 96 pid.SetOutputLimits(-255, 255); 97 } 98 else 99 { 100 // ERROR! 101 // 1 = initial memory load failed 102 // 2 = DMP configuration updates failed 103 // (if it's going to break, usually the code will be 1) 104 Serial.print(F("DMP Initialization failed (code ")); 105 Serial.print(devStatus); 106 Serial.println(F(")")); 107 } 108} 109 110 111void loop() 112{ 113 // if programming failed, don't try to do anything 114 if (!dmpReady) return; 115 116 // wait for MPU interrupt or extra packet(s) available 117 while (!mpuInterrupt && fifoCount < packetSize) 118 { 119 //no mpu data - performing PID calculations and output to motors 120 pid.Compute(); 121 motorController.move(output, MIN_ABS_SPEED); 122 123 } 124 125 // reset interrupt flag and get INT_STATUS byte 126 mpuInterrupt = false; 127 mpuIntStatus = mpu.getIntStatus(); 128 129 // get current FIFO count 130 fifoCount = mpu.getFIFOCount(); 131 132 // check for overflow (this should never happen unless our code is too inefficient) 133 if ((mpuIntStatus & 0x10) || fifoCount == 1024) 134 { 135 // reset so we can continue cleanly 136 mpu.resetFIFO(); 137 Serial.println(F("FIFO overflow!")); 138 139 // otherwise, check for DMP data ready interrupt (this should happen frequently) 140 } 141 else if (mpuIntStatus & 0x02) 142 { 143 // wait for correct available data length, should be a VERY short wait 144 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 145 146 // read a packet from FIFO 147 mpu.getFIFOBytes(fifoBuffer, packetSize); 148 149 // track FIFO count here in case there is > 1 packet available 150 // (this lets us immediately read more without waiting for an interrupt) 151 fifoCount -= packetSize; 152 153 mpu.dmpGetQuaternion(&q, fifoBuffer); 154 mpu.dmpGetGravity(&gravity, &q); 155 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 156 input = ypr[1] * 180/M_PI + 180; 157 } 158} 159
Downloadable files
circuit diagram
connect the parts as shown in figure
circuit diagram
circuit diagram
connect the parts as shown in figure
circuit diagram
Comments
Only logged in users can leave comments
SAFVANOKP
0 Followers
•0 Projects
Table of contents
Intro
7
0