TÀI LIỆU THAM KHẢO
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (994.71 KB, 42 trang )
ĐỒ ÁN TỐT NGHIỆP
Trang 38/45
[3]
Kiều Xuân Thực (2009); Vi điều khiển cấu trúc - lập trình và ứng dụng;
NXB giáo dục Việt Nam.
[4]
Phan Đình Duy (2016), Vũ Đức Lung, Lê Quang Minh; NXB Đại Học
Quốc Gia, Hồ Chí Minh.
[5]
Nguyễn Thị Phương Hà (1999); Lý thuyết điều khiển tự động; NXB
Khoa học và kỹ thuật, Hà Nội.
[6]
Nguyễn Thị Phương Hà (1999); Lý thuyết điều khiển tự động; NXB
Khoa học và kỹ thuật, Hà Nội.
[7]
Nguyễn Thị Phương Hà (1999); Lý thuyết điều khiển tự động; NXB
Khoa học và kỹ thuật, Hà Nội.
[8]
Nguyễn Dỗn Phước (2002); Lý thuyết điều khiển tuyến tính; NXB khoa
học và kỹ thuật, Hà Nội.
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 39/45
PHỤ LỤC A
#CODE XE CÂN BẰNG
#include
#include
#include
#include
#include
MPU6050 CBgoc;
Kalman kalman;
const int DHTPIN =2;
const int DHTTYPE=DHT11;
DHT dht(DHTPIN,DHTTYPE);
int16_t ax,ay,az;
int16_t gx,gy,gz;
double dt;
double last_time;
double CurrentAngle;
double set_point = 0;
double kp = 23 ; //23 //22//22//23
double ki = 0.5 ; //0 //0.2//0.2//0.5
double kd = 2.2 ; //1.15//2//2.2//2.2
double p,i,d;
double e, last_e;
double output;
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 40/45
char blue;
char status = '0';
int AIN1 = 8;
int AIN2 = 11;
int BIN1 = 12;
int BIN2 = 13;
int CIN1 = 9;
int CIN2 = 10;
int speed;
int speed1;
int spd = 100;
void setup()
{
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(CIN1, OUTPUT);
pinMode(CIN2, OUTPUT);
Serial.begin(9600);
Wire.begin();
CBgoc.initialize();
}
void loop()
{
cambiengoc();
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 41/45
temphumi();
if(CurrentAngle > -80 && CurrentAngle < 80)
{
pid();
android();
}
else
{
stop();
}
}
void pid()
{
double samplingtime = 0.01;
e = set_point - CurrentAngle;
p = e*kp;
i += e*ki*samplingtime;
d = (kd*(e - last_e))/samplingtime;
output = p+i+d;
last_e = e;
speed = constrain(output, -255, 255);
if(status == '0')
{
if(blue == 'S')
{
set_point = 0;
}
else if(blue == 'U')
{
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 42/45
set_point = 8;
}
else if(blue == 'D')
{
set_point = -8;
}
if(blue == 'L' && CurrentAngle > -3 && CurrentAngle < 3)
{
trai();
}
else if(blue == 'R' && CurrentAngle > -3 && CurrentAngle < 3)
{
phai();
}
else if(CurrentAngle > -80 && CurrentAngle < 80)
{
motor();
}
}
else if(status == '1')
{
motor();
}
}
void motor()
{
if(speed >= 0)
{
analogWrite(CIN1, speed);
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 43/45
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
else
{
speed = map(speed,0,-255,0,255);
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
}
void stop()
{
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
void cambiengoc()
{
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 44/45
CBgoc.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ay=(ay/182);
gx=(gx/131);
CurrentAngle=kalman.getAngle(ay,gx,dt);
dt=(millis()-last_time)/1000.0;
last_time=millis();
}
void trai()
{
speed1 = constrain((speed+100), -255, 255);
analogWrite(CIN1,abs(speed1));
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2,abs(speed1));
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
void phai()
{
speed1 = constrain((speed+100), -255, 255);
analogWrite(CIN1,abs(speed1));
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(CIN2,abs(speed1));
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 45/45
void android()
{
if(Serial.available() > 0)
{
blue = Serial.read();
if(Serial.read() == '1')
{
status = '1';
}
}
}
void temphumi()
{
if (blue=='T')
{
float h =dht.readHumidity();
float t =dht.readTemperature();
Serial.print("Temperature: ");
Serial.println(t);
Serial.print("Humidity: ");
Serial.println(h);
while (blue=='T')
{
pid();
android();
}
}
}
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 46/45
#CODE ĐIỀU KHIỂN BLUETOOTH
Xe tự cân bằng điều khiển bluetooth
ĐỒ ÁN TỐT NGHIỆP
Trang 47/45
Xe tự cân bằng điều khiển bluetooth
