Tải bản đầy đủ - 0 (trang)
PHỤ LỤC 1: Code chương trình file Main.ino điều khiển trên Arduino.

PHỤ LỤC 1: Code chương trình file Main.ino điều khiển trên Arduino.

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

void Quaytrai();

void CheckStart();

void CheckStop();

void ProcessPoint();

void MPU6050();

void Init_MPU6050();

void Laser_Processing();

/* Function Data */

String sdata;

bool balanceOK = false ;

/* Servo Data */

int AngleServo = 0;

bool AngleOverMax = false;

bool AngleOverMin = true;

int countindex = 0;

/* Laser Data */

Int Laser[30] = {22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,

44,45,46,47,48,49,50,51};

bool laserOK = false;

int numlaser;

/* IMU Data */

double accX, accY, accZ;

double gyroX, gyroY, gyroZ;

int16_t tempRaw;

uint32_t timer;

uint8_t i2cData[14]; // Buffer for I2C data

/*................................... void setup() ...................................*/

void setup()

{Serial.begin(9600);

Serial2.begin(9600);

InitBalanceDC();

InitMove();

pinMode(LED,OUTPUT);

53



InitLaser();

Init_MPU6050();

while(!StartOK)

{

point = 0;

CheckStart();

}

}

/*................................... void loop() ...................................*/

void loop()

{

while(!StopOK)

{ DichuyenOK = false;

while(!DichuyenOK)

{ ProcessPoint();

point++;

if(point>Maxpoint)

{ point = 0;

StopOK = true;}

DichuyenOK = true;}

CheckStop();}

CheckStart();}



54



PHỤ LỤC 2: Code Arduino phần kiểm tra tín hiệu Start và Stop.

/*************************** CheckStart() ***************************/

void CheckStart()

{

if(Serial2.available()>0)

{

sdata = Serial2.readStringUntil(',');

if(sdata == "Start")

{

StartOK = true;

StopOK = false;

}

}

}

/*************************** CheckStop() ***************************/

void CheckStop()

{ if(Serial2.available()>0)

{



sdata = Serial2.readStringUntil(',');

if(sdata == "Stop")

{

StartOK = false;

StopOK = true;

}



}

55



}



56



PHỤ LỤC 3: Code Arduino giao tiếp I2C với module MPU6050.

const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB

const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C

communication



uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {

return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success

}



uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop)

{

Wire.beginTransmission(IMUAddress);

Wire.write(registerAddress);

Wire.write(data, length);

uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success

if (rcode) {

Serial.print(F("i2cWrite failed: "));

Serial.println(rcode);

}

return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission

}



uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {

uint32_t timeOutTimer;

Wire.beginTransmission(IMUAddress);

57



Wire.write(registerAddress);

uint8_t rcode = Wire.endTransmission(false); // Don't release the bus

if (rcode) {

Serial.print(F("i2cRead failed: "));

Serial.println(rcode);

return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission

}

Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and

then release the bus after reading

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

if (Wire.available())

data[i] = Wire.read();

else {

timeOutTimer = micros();

while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());

if (Wire.available())

data[i] = Wire.read();

else {

Serial.println(F("i2cRead timeout"));

return 5; // This error value is not already taken by endTransmission

}

}

}

return 0; // Success

}

58



PHỤ LỤC 4: Code Arduino bộ phận cảm biến dò tia laser.

void InitLaser()

{ pinMode(CTTrai,INPUT);

pinMode(CTPhai,INPUT);

for(int i = 0;i++;i<30)

{ pinMode(Laser[i],INPUT); }}

void Laser_Processing()

{ laserOK = false;

while(!laserOK)

{



if(digitalRead(CTTrai)==1)

{ Myservo.write(57);

}

if(digitalRead(CTPhai)==1)

{ Myservo.write(85);

}

if(digitalRead(Laser[numlaser]))

{Myservo.write(70);

laserOK = true;

}

numlaser ++;

if(numlaser > 30)

{



numlaser = 0;



}

}

59



}



60



PHỤ LỤC 5: Code Arduino bộ phận cân bằng.

void InitBalanceDC()

{

pinMode(L298IN1,OUTPUT);

pinMode(L298IN2,OUTPUT);

pinMode(L298IN3,OUTPUT);

pinMode(L298IN4,OUTPUT);

}

/*................................... void MPU6050() ...................................*/

void MPU6050()

{

balanceOK = false;

while(!balanceOK)

{

/* Update all the values */

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

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

accY = ((i2cData[2] << 8) | i2cData[3]);

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

tempRaw = (i2cData[6] << 8) | i2cData[7];



double dt = (double)(micros() - timer) / 1000000; // Calculate delta time

timer = micros();

#ifdef RESTRICT_PITCH // Eq. 25 and 26

61



double roll = atan2(accY, accZ) * RAD_TO_DEG;

double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

#else // Eq. 28 and 29

double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

double pitch = atan2(-accX, accZ) * RAD_TO_DEG;

#endif



if (int(roll) == 0)

{

analogWrite(L298IN1,0);

analogWrite(L298IN2,0);

delay(1);

if ((int(pitch) == 0))

{

analogWrite(L298IN3,0);

analogWrite(L298IN4,0);

delay(1);

Myservo.attach(PinServo);

Myservo.write(85);

balanceOK = true;

}



else if(int(pitch) < 0)

{

analogWrite(L298IN3,60);

62



analogWrite(L298IN4,0);

delay(1);

}



else if(int(pitch) > 0)

{

analogWrite(L298IN3,0);

analogWrite(L298IN4,60);

delay(1);

}

}



else if(int(roll) < 0)

{

analogWrite(L298IN1,60);

analogWrite(L298IN2,0);

delay(1);

}



else if(int(roll) > 0)

{

analogWrite(L298IN1,0);

analogWrite(L298IN2,60);

delay(1);

}

63



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

PHỤ LỤC 1: Code chương trình file Main.ino điều khiển trên Arduino.

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

×