Tải bản đầy đủ - 0 (trang)
1 Những kết quả đạt được

1 Những kết quả đạt được

Tải bản đầy đủ - 0trang

TÀI LIỆU THAM KHẢO

http://codientu.org/threads/6894/

http://www.hocavr.com/index.php/app/dcservo#5-PID

http://tienanh.info/hoc-tap/co-dien-tu/dieu-khien-xe-2-banh-tu-can-bang-01-052014.html

https://bayesianadventures.wordpress.com/2013/10/27/balance-good-everythinggood/

http://www.ee.usyd.edu.au/tutorials_online/matlab/examples/pend/invPID.html

http://www.instructables.com/id/2-Wheel-Self-Balancing-Robot-by-using-Arduinoand-/

http://ctms.engin.umich.edu/CTMS/index.php?

example=InvertedPendulum§ion=ControlPID

http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pidintroduction/

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-andhow-to-implement-it

http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit

http://www.khoahocphothong.com.vn/news/detail/11059/che-tao-hoan-chinh-xehai-banh-tu-can-bang.html



PHỤ LỤC

#include

#include "Kalman.h"

#define RESTRICT_PITCH

Kalman kalmanX;

Kalman kalmanY;

#define STD_LOOP_TIME 10000

unsigned long lastLoopUsefulTime = STD_LOOP_TIME;

unsigned long loopStartTime;

/* IMU Data */

int16_t accX, accY, accZ;

int16_t tempRaw;

int16_t gyroX, gyroY, gyroZ;

/*int16_t accXzero, accYzero, accZzero;

int16_t gyroXzero, gyroYzero, gyroZzero;*/

double accXzero,accZzero,gyroYzero;

double gyroYrate;

double accXangle, accYangle;

double gyroYangle;

double kalAngleX, kalAngleY;

float pTerm=0;

float iTerm=0;

float dTerm=0;

uint32_t timer;

uint8_t i2cData[14];

int rotate_left=0;

// turn left



int rotate_right=0;

float error=0;

float errorS=0.0f;

float errord=0.0f;

float lastangle=0;

float llastangle=0;

float Kp =165.0f; //85

float Kd =50.00f; //15

float Ki = 160.00f; ;//50 //

float target = 181;

float errorCurrent = 0.0f;

float errorLast = 0.0f;

float errorSum = 0.0f;

float errorDelta = 0.0f;

byte ComStep=0;

unsigned long timeLast = 0;

char command ='S';

const int MotorLeftPin1 = 4;

const int MotorLeftPin2 = 5;

const int MotorLeftPinEn = 10; //PWM enabled on the Uno

float outl=0;

float outr=0;

float out=0;

const int MotorRightPin1 = 6;

const int MotorRightPin2 = 7;

const int MotorRightPinEn = 11; //PWM enabled on the Uno

const int minSpeed = 70; // defines the useful PWM range of the motors [minSpeed,

255]

char prevCommand='A';



void setup() {

Serial.begin(9600);

Wire.begin();

TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro

filtering, 8 KHz sampling

i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s

i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g

while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once

while (i2cWrite(0x6B, 0x09, true)); // PLL with X axis gyroscope reference and

disable sleep mode

delay(400);

// Wait for sensor to stabilize

/* Set kalman and gyro starting angle */

while (i2cRead(0x3B, i2cData, 6));

accX = (i2cData[0] << 8) | i2cData[1];

accZ = (i2cData[4] << 8) | i2cData[5];

// It is then converted from radians to degrees

accYangle = (atan2(accX-accXzero,accZ-accZzero)+PI)*RAD_TO_DEG;

kalmanY.setAngle(accYangle);

gyroYangle = accYangle;

pinMode(MotorLeftPin1, OUTPUT);

pinMode(MotorLeftPin2, OUTPUT);

pinMode(MotorLeftPinEn, OUTPUT);

pinMode(MotorRightPin1, OUTPUT);

pinMode(MotorRightPin2, OUTPUT);

pinMode(MotorRightPinEn, OUTPUT);

timer = micros();

}



void loop()

{

calibrateGyro(); //calib cam bien

calibrateAcc(); //

//Doc gia tri tho;

while (i2cRead(0x3B, i2cData, 12));// doc thanh ghi co dia chi bat dau la 0x3B,doc

12 byte,gia tri luu vao i2cbuffer

accX = ((i2cData[0] << 8) | i2cData[1]);//gia tri accX 16bit

accZ = ((i2cData[4] << 8) | i2cData[5]);

gyroY = ((i2cData[10] << 8) | i2cData[11]);

accYangle

=

(atan2((double)accX-accXzero,

(double)accZ-accZzero)

+PI)*RAD_TO_DEG;// do goc nghieng dua vao Acc, tu 0 den 2pi,doi ra do.

gyroYrate = -((double)gyroY-gyroYzero)/131.0;//gia tri tho(raw), chia 131 se ra gia

tri toc do goc xoay quanh truc y,dau tru de phu hop chieu cua goc.

gyroYangle += gyroYrate*((double)(micros() - timer)/1000000);//goc tinh tu gyro;

kalAngleY = kalmanY.getAngle(accYangle, gyroYrate,

timer)/1000000); // Calculate the angle using a Kalman filter

loopStartTime = micros();

timer = micros(); //Re_setup timer for Kalman fiter;

if(Serial.available()>0){

prevCommand=command;

command=Serial.read();

if(command!=prevCommand){

switch(command){

case'S':

target=180;

rotate_left=0;

rotate_right=0;

break;

case'F':



(double)(micros()



-



target=180+ComStep*0.2;

rotate_left=0;

rotate_right=0;

break;

case'B':

target=180-ComStep*0.2;

rotate_left=0;

rotate_right=0;

break;

case'L':

rotate_left=0;

rotate_right=ComStep*10;

break;

case'R':

rotate_left=ComStep*10;

rotate_right=0;

break;

case'1':

ComStep=1;

break;

case'2':

ComStep=2;

break;

case'3':

ComStep=3;

break;

case '4':

ComStep=4;

break;



case '5':

ComStep=5;

break;

case '6':

ComStep=6;

break;

case '7':

ComStep=7;

break;

case '8':

ComStep=8;

break;

case '9':

ComStep=9;

break;

default:

;

}

}

}

if ((int)kalAngleY >=150 && kalAngleY <= 210) {

out=pid();

outl=out + rotate_left;

outr=out + rotate_right;

drive(outl,outr);

}

else{

out=0;

outl=0;



outr=0;

drive(outl,outr);

}

/* Use a time fixed loop 10ms*/

lastLoopUsefulTime = micros() - loopStartTime;

if (lastLoopUsefulTime < STD_LOOP_TIME)

delayMicroseconds(STD_LOOP_TIME - lastLoopUsefulTime);

loopStartTime = micros();

Serial.print("goc nghieng :");

Serial.println(error);

}

int pid(){

unsigned long now = millis();

float timeDelta = (float)(now - timeLast);

error=target-kalAngleY;

errorS=kalAngleY-lastangle;

errord=(kalAngleY-2*lastangle+llastangle)/timeDelta;

float pTerm=Kp*errorS;

float iTerm=Ki*timeDelta*error;

float dTerm=Kd*errord;

float limit = 255.0f;

if (iTerm > limit) {iTerm = limit;}

else if (iTerm < -limit) {iTerm = -limit;}

float pid= pid - pTerm + iTerm - dTerm;

lastangle=kalAngleY;

llastangle=lastangle;

timeLast=now;

int out=(int)pid*(-1);

return out;



}

void drive(int leftMotorSpeed, int rightMotorSpeed) {

int left = constrain(leftMotorSpeed, -255, 255);

int right = constrain(rightMotorSpeed, -255, 255);

if (left < 0) {

if (left >= -40) {left = 0;}

else if (left >= -80) {left = -60;}

else if (left < -80) {left =left+30;}

digitalWrite(MotorLeftPin2, LOW);

digitalWrite(MotorLeftPin1, HIGH);

analogWrite(MotorLeftPinEn, -left);

}

else if (left == 0) {

digitalWrite(MotorLeftPin1, LOW);

digitalWrite(MotorLeftPin2, LOW);

digitalWrite(MotorLeftPinEn, LOW);

}

else {

if (left <= 40) {left = 0;}

else if (left <=80) {left = 60;}

else if (left > 80) {left =left-30;}

digitalWrite(MotorLeftPin2, HIGH);

digitalWrite(MotorLeftPin1, LOW);

analogWrite(MotorLeftPinEn, left);

}

if (right < 0) {

if (right >= -40) {right = 0;}

else if (right >=-90) {right = -90;}

digitalWrite(MotorRightPin2, LOW);



digitalWrite(MotorRightPin1, HIGH);

analogWrite(MotorRightPinEn, -right);

}

else if (right == 0) {

digitalWrite(MotorRightPin1, LOW);

digitalWrite(MotorRightPin2, LOW);

digitalWrite(MotorRightPinEn, LOW);

}

else {

if (right <= 40) {right = 0;}

else if (right <= 90) {right = 90;};

digitalWrite(MotorRightPin2, HIGH);

digitalWrite(MotorRightPin1, LOW);

analogWrite(MotorRightPinEn, right);

}

}

void calibrateAcc() {

// while (Serial.read() == -1);

Serial.println("Starting calibration");

int16_t accXbuffer[50], accZbuffer[50];

for (uint8_t i = 0; i < 50; i++) {

while (i2cRead(0x3B, i2cData, 6));

accXbuffer[i] = ((i2cData[0] << 8) | i2cData[1]);

accZbuffer[i] = ((i2cData[4] << 8) | i2cData[5]);

delay(10);

}

if (!checkMinMax(accXbuffer, 50, 1000) || !checkMinMax(accZbuffer, 50, 1000)) {

Serial.print("Accelerometer calibration error");

//digitalWrite(buzzer, HIGH);



while (1); // Halt

}

for (uint8_t i = 0; i < 50; i++) {

accXzero += accXbuffer[i];

accZzero += accZbuffer[i];

}

accXzero /= 50;

accZzero /= 50;

if (accZzero < 0)

accZzero += 16384.0;

else

accZzero -= 16384.0;

Serial.print("Zero values: ");

Serial.print(accXzero);

Serial.print(",");

Serial.println(accZzero);

Serial.print("New zero values (g's): ");

Serial.print(accXzero/16384.0);

Serial.print(",");

Serial.println(accZzero/16384.0);

Serial.println("Calibration of the accelerometer is done");

}

void calibrateGyro() {

int16_t gyroYbuffer[50];

for (uint8_t i = 0; i < 50; i++) {

while (i2cRead(0x45, i2cData, 2));

gyroYbuffer[i] = ((i2cData[0] << 8) | i2cData[1]);

delay(10);

}



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

1 Những kết quả đạt được

Tải bản đầy đủ ngay(0 tr)

×