Projede kullanılacak malzemeler:
Bakır plaket
Arduino nano
MPU-6050 Gyro sensör
TB-6612 motor sürücü
HC-04 bluetooth modül
2 adet 7805 entegre
3 adet klemens
Anahtar
Dişi header
2 adet 12v mini dc motor
2 adet 100mm çapında 3mm mil girişi olan tekerlek.
Devre tasarımı: Proteus ares programı kullanılarak aşağıda ki devre çizilir.
Devre şeması bakır plaketin üzerine ütü kullanılarak geçirilir.
Bakır plaket Hidrojen peroksit ve perhidrol çözeltisine atılır ve baskı devre tamamlanır.
Devrenin üzerine gelecek olan dişi header, klemens gibi devre elemanların yuvaları matkap kullanılarak açılır.
Yuvalara gerekli malzemeler yerleştirildikten sonra lehimlenir.
Devreye devre elemanları monte edildikten sonra arduinoya robotu çalıştıracak olan program yüklenir.
#include “I2Cdev.h”
#include “MPU6050_6Axis_MotionApps20.h”
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include “Wire.h”
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#include <SoftwareSerial.h>
SoftwareSerial BT(4,5);
bool blinkState = false;
int baslat=0;
double P = 500 , D = 15000 , I = 2 ;
double P_yaw =2000 , D_yaw =0 , I2 = 0 ;
#define AIL LOW
#define AILPIN 2
#define AILMID 1500
#define ROLL HIGH
#define ROLLPIN 3
#define ROLLMID 1500
volatile int AILin = AILMID;
volatile unsigned long StartAIL = 0;
volatile boolean NewAIL = false;
volatile int ROLLin = ROLLMID;
volatile unsigned long StartROLL = 0;
volatile boolean NewROLL = false;
unsigned long sondurum;
double Ihata, sonhata, hata, output, ref=0, Iout;
unsigned long sondurum2;
double Ihata2, sonhata2, hata2, outputyaw, ref2, Iout2,yaw_last_time;
float roll, aileron, kumandaail, pitch, elevator, kumandaelev, in,yaw,yaw_angle;
double motor1 = 0 , motor2 = 0 ;
double output1, output2, sagsol=0;
const int pwm_sol = 10;
const int solmotor_in1 = 7;
const int solmotor_in2 = 8;
const int sagmotor_in1 = 11;
const int sagmotor_in2 = 9;
const int pwm_sag = 6;
// MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
// orientation/motion vars
Quaternion q;
VectorFloat gravity;
float ypr[3];
int something;
volatile bool mpuInterrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() { // Serial.begin(9600);
BT.begin(9600);
pinMode(solmotor_in1, OUTPUT);
pinMode(solmotor_in2, OUTPUT);
pinMode(sagmotor_in1, OUTPUT);
pinMode(sagmotor_in2, OUTPUT);
pinMode(pwm_sol, OUTPUT);
pinMode(pwm_sag, OUTPUT);
pinMode(13, OUTPUT);
// TCCR0B=TCCR0B & B11111000 | B00000010;
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
while (!Serial);
Serial.println(F(“Initializing I2C devices…”));
mpu.initialize();
Serial.println(F(“Testing device connections…”));
Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));
Serial.println(F(“\nSend any character to begin DMP programming and demo: “));
Serial.println(F(“Initializing DMP…”));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus == 0) {
Serial.println(F(“Enabling DMP…”));
mpu.setDMPEnabled(true);
Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F(“DMP ready! Waiting for first interrupt…”));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
{
attachInterrupt(AIL, AILhesapla, CHANGE);
attachInterrupt(ROLL, ROLLhesapla, CHANGE);
}
}
void loop() {
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Serial.println(ypr[1] * 180/M_PI);
something = (ypr[1] * 180 / M_PI);
}
/////***********************************************************************************************
//
//if (ypr[2]<-1)
//{
// dur();
// Ihata=0; Iout=0;
// Serial.println(“robotu dik konuma getirin”);
// goto bekle;
//
//}
//
// if (ypr[2]>1)
//{
// dur();
// Ihata=0; Iout=0;
// Serial.println(“robotu dik konuma getirin”);
// goto bekle;
//
//}
//if(baslat){
PID();
PID_yaw();
motorayar();//}
// else {
// analogWrite(pwm_sag,0);
// analogWrite(pwm_sol,0);
//}
Bluetooth();
ekranpid();
}
//////////*******************************************************************************************
void PID()
{
unsigned long simdi = millis();
double zamandegisimi = (double)(simdi – sondurum);
double hata = 0.01-ypr[1]+ref;
Ihata += (hata * zamandegisimi);
if (Ihata<-500) Ihata=-500;
if (Ihata>500) Ihata=500;
Iout = I * Ihata;
int L = 250;
if (Iout < -L) Iout = -L;
if (Iout > L) Iout = L;
double dhata = (hata – sonhata) / zamandegisimi;
output = ( P * hata + Iout + D * dhata);
if (output > 250) output = 250;
if (output < -250) output = -250;
sonhata = hata;
sondurum = simdi;
}
void PID_yaw()
{
yaw = ypr[0];
yaw_angle = (float)(yaw – yaw_last_time);
unsigned long simdi = millis();
double zamandegisimi = (double)(simdi – sondurum2);
double hata2 = 0 – yaw_angle+sagsol;
Ihata2 += (hata2 * zamandegisimi);
if (Ihata2<-250) Ihata2=-250;
if (Ihata2>250) Ihata2=250;
Iout2 = I2 * Ihata2;
int L = 250;
if (Iout2 < -L) Iout2 = -L;
if (Iout2 > L) Iout2 = L;
double dhata2 = (hata2 – sonhata2) / zamandegisimi;
outputyaw = ( P_yaw * hata2 + Iout2 + D_yaw * dhata2);
if (outputyaw > 250) outputyaw = 250;
if (outputyaw < -250) outputyaw = -250;
sonhata2 = hata2;
sondurum2 = simdi;
yaw_last_time = yaw;
}
/////////////////////****************************************************************************************
void motorkontrol()
{
if (output > 250) output = 250;
if (output < -250) output = -250;
if (output <= 0)
{
output2 = 0;
output1 = -output;
duz(); analogWrite(pwm_sol, output1); analogWrite(pwm_sag, output1);
}
if (output > 0)
{
output1 = 0;
output2 = output;
geri(); analogWrite(pwm_sol, output2 ); analogWrite(pwm_sag, output2);
}
}
/////////////**********************************************************************************************
void motorayar()
{
motor1=output+outputyaw;
motor2=output-outputyaw;
if (motor1>0) digitalWrite(13,1);
if (motor1<0) digitalWrite(13,0);
if (motor1<-250) {motor1=-250;}
if (motor1>250){ motor1=250;}
if (motor2<-250) {motor2=-250;}
if (motor2>250){ motor2=250;}
/////******************************************
if (motor1 <= 0)
{
motor1 = -motor1;
sol_geri(); analogWrite(pwm_sol, motor1);
}
else if (motor1 > 0)
{
motor1 =motor1;
sol_ileri(); analogWrite(pwm_sol, motor1 );
}
/////******************************************
if (motor2 <= 0)
{
motor2 = -motor2;
sag_geri(); analogWrite(pwm_sag, motor2);
}
else if (motor2 > 0)
{
motor2 =motor2;
sag_ileri(); analogWrite(pwm_sag, motor2 );
}
////*********************************************
}
void ekranpid()
{
Serial.print(” pitch = “); Serial.print(ypr[1]);
Serial.print(” roll = “); Serial.print(ypr[2]);
Serial.print(” yawng= “); Serial.print(yaw_angle);
// Serial.print(” ref = “); Serial.print(ref);
Serial.print(” ROLLin = “); Serial.print(ROLLin);
// Serial.print(” sagsol = “); Serial.print(sagsol);
Serial.print(” output = “); Serial.print(output);
Serial.print(” outputyaw = “); Serial.print(outputyaw);
Serial.print(” motor1 = “); Serial.print(motor1);
Serial.print(” motor2 = “); Serial.print(motor2);
Serial.println();
}
//////*********************************************************************************************************
void duz() {
sag_ileri();
sol_ileri();
}
void geri() {
sag_geri();
sol_geri();
}
void sag_ileri() {
digitalWrite(sagmotor_in1, LOW);
digitalWrite(sagmotor_in2, HIGH);
}
void sag_geri() {
digitalWrite(sagmotor_in1, HIGH);
digitalWrite(sagmotor_in2, LOW);
}
void sol_ileri() {
digitalWrite(solmotor_in1, LOW);
digitalWrite(solmotor_in2, HIGH);
}
void sol_geri() {
digitalWrite(solmotor_in1, HIGH);
digitalWrite(solmotor_in2, LOW);
}
void dur() {
analogWrite(pwm_sol, 0);
analogWrite(pwm_sag, 0);
}
/////////******************************************************************************************
void AILhesapla()
{
if (digitalRead(AILPIN) == HIGH)
{
StartAIL = micros();
}
else
{
if (StartAIL && (NewAIL == false))
{
AILin = (int)((micros()) – StartAIL);
StartAIL = 0;
NewAIL = true;
}
}
}
//////////*******************************************************************************
void ROLLhesapla()
{
if (digitalRead(ROLLPIN) == HIGH)
{
StartROLL = micros();
}
else
{
if (StartROLL && (NewROLL == false))
{
ROLLin = (int)((micros()) – StartROLL);
StartROLL = 0;
NewROLL = true;
}
}
}
void Bluetooth(){
if(BT.available()){
char gelen =((BT.read()));
switch (gelen){
case ‘a’ : ref+=0.1; break;
case ‘b’ : ref-=0.1; break;
case ‘c’ : sagsol+=0.01; break;
case ‘d’ : sagsol-=0.01; break;
case ‘0’ : ref=0; sagsol=0; break;
case ‘9’ : baslat=0; break;
case ‘8’ : baslat=1; break;
// case ‘a’ : sagcarpan+=0.01; BT.print(sagcarpan); BT.println(); break;
// case ‘a’ : sagcarpan-=0.01; BT.print(sagcarpan); BT.println(); break;
}
}}