Scribd
Upload
49 views2 pages

Control

This document contains code for controlling a motor based on gyroscope readings from an MPU-6050 sensor. It initializes pin connections for motor control and I2C communication, reads gyroscope data from the sensor, maps the gyroscope values to a PWM output to control motor speed, and prints the gyroscope values for debugging. The motor direction and speed are adjusted based on changes in the gyroscope reading to stabilize rotation.

Uploaded by

Đình Cường
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
49 views2 pages

Control

This document contains code for controlling a motor based on gyroscope readings from an MPU-6050 sensor. It initializes pin connections for motor control and I2C communication, reads gyroscope data from the sensor, maps the gyroscope values to a PWM output to control motor speed, and prints the gyroscope values for debugging. The motor direction and speed are adjusted based on changes in the gyroscope reading to stabilize rotation.

Uploaded by

Đình Cường
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/ 2

#include<Wire.

h>
int in3 = 5;
int in4 = 4;
int enB = 3; //B
int enA = 8; //A
int in1 = 7;
int in2 = 6;
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t GyY; // declare accellerometer and gyro variables
int16_t A=0;
int16_t B=0;
int time;
int S;
void setup(){
pinMode(enA, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB,OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
Wire.begin(); // initiate i2c system
Wire.beginTransmission(MPU_addr); // be sure we talk to our MPU vs some other
device
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true); // done talking over to MPU device, for the moment
Serial.begin(2000000); // initialize serial port to 9600 bps so you can see your
debug messages in Arduino IDE via debug channel
}
void loop(){ // main program loop
Wire.beginTransmission(MPU_addr); // get ready to talk to MPU again
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false); // done talking to MPU for the time being
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
// all the fancy <<8| stuff is to bit shift the first 8 bits to
// the left & combine it with the next 8 bits to form 16 bits

GyY=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)


// the above lines have gathered Accellerometer values for X, Y, Z
// as well as Gyroscope values for X, Y, Z

; //Control

A=GyY/100;

if((A>0)&&((A-B)>2)){ // N?u A tang th� tang i, n�n


digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
S = map(A,0,200,0,255);
analogWrite(enB, S);
}
if((A<0)&&((A-B)<-2)){ // N?u A gi?m th� tang i, d?y
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
S = map(A,0,-200,0,255);
analogWrite(enB, S);
}
if(-2<(A-B)<2){
time=millis();
int duration = millis() - time; // do th?i gian duy tr� tr?ng th�i hi?n t?i
duration = constrain(duration,0,1000); // 0 <= duration <= 1000 ms.
S = map(A,0,200,255,0);
analogWrite(enB,S); // xu?t xung PWM d? di?u khi?n t?c d? motor
// duty cycle c�ng l?n th� motor quay c�ng
nhanh
}

B = A;
Serial.println (A);
delay(50); // delay a bit to not overwhelm you the user/programmer as you view
the results
}

You might also like