Android Kontrollü Denge Robotu

Anasayfa ›› Arduino Robotlarım ›› Android Kontrollü Denge Robotu

Daha önce yapmış olduğum denge robotunu bu seferde Android Kontrollü yapmak istedim . Kullanımı biraz zor olsa da sonunda eliniz de alışınca kontrol etmesi zevkli bir proje oldu. Adım Adım nasıl yapıldığını videodan izleyebilirsiniz.

APK dosyasını linkten indiriniz.

https://drive.google.com/open?id=0B0YUEN70aW3QM3RPUndGQ0dsNEE

#include <Arduino.h>
#include <avr/wdt.h>
#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

#define MANUAL_TUNING 1
#define MOVE_BACK_FORTH 1

double MIN_ABS_SPEED=15;
String id = "";                                     
String stvalue = "";
double value;
int qq=0;

MPU6050 mpu;                                         

bool dmpReady = false;                               
uint8_t mpuIntStatus;                                
uint8_t devStatus;                                   
uint16_t packetSize;                                
uint16_t fifoCount;                                  
uint8_t fifoBuffer[64];                              

Quaternion q;                                        
VectorFloat gravity;                                 

float ypr[3];                                        
double kp=35 , ki=190, kd=1.0;                       //PID Değerleri buraya yazılacak
double prevKp, prevKi, prevKd;
double originalSetpoint = 174.80;//169,99 <  175,00 >
double setpoint = originalSetpoint;
double movingAngleOffset = 3.3;
double input, output;
int moveState=0;                                     

#if MANUAL_TUNING
PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
PID pid(&input, &output, &setpoint, 35, 190, 1, DIRECT);  //Mobil uygulamadan gönderilen otomatik PID değerleri buraya yazılacak
#endif
//MOTOR CONTROLLER

//int ENA = 5;//Motor pin bağlantıları
//int IN1 = 6;//Motor pin bağlantıları
//int IN2 = 7;//Motor pin bağlantıları
//int IN3 = 8;//Motor pin bağlantıları
//int IN4 = 9;//Motor pin bağlantıları
//int ENB = 10;//Motor pin bağlantıları

//int ENA = 3;//Motor pin bağlantıları
//int IN1 = 5;//Motor pin bağlantıları
//int IN2 = 6;//Motor pin bağlantıları
//
//
//int ENB = 11;//Motor pin bağlantıları
//int IN3 = 8;//Motor pin bağlantıları
//int IN4 = 9;//Motor pin bağlantıları


int mavi=13;
int kirmizi=12;

int ENA = 3;//Motor pin bağlantıları
int IN1 = 4;//Motor pin bağlantıları
int IN2 = 5;//Motor pin bağlantıları


int ENB = 11;//Motor pin bağlantıları
int IN3 = 9;//Motor pin bağlantıları
int IN4 = 8;//Motor pin bağlantıları


LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 10,10);

//timers
long time1Hz = 0;
long time5Hz = 0;

volatile bool mpuInterrupt = false;               //

void dmpDataReady(){
mpuInterrupt = true;
}

void setup(){
wdt_disable();
pinMode(7,OUTPUT);     
pinMode(mavi,OUTPUT);            
pinMode(kirmizi,OUTPUT);                                        
digitalWrite(7,HIGH);

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE  
Wire.begin();
TWBR = 24;                                    
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

Serial.begin(9600);
mpu.initialize();
devStatus = mpu.dmpInitialize();              
mpu.setXGyroOffset(150);          //MPU6050 Offset Değerleri
mpu.setYGyroOffset(30);           //MPU6050 Offset Değerleri
mpu.setZGyroOffset(-85);          //MPU6050 Offset Değerleri
mpu.setZAccelOffset(1788);

if (devStatus == 0){                          
mpu.setDMPEnabled(true);                      
attachInterrupt(0, dmpDataReady, RISING);     
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;                              
packetSize = mpu.dmpGetFIFOPacketSize();      
pid.SetMode(AUTOMATIC);                       
pid.SetSampleTime(5);
pid.SetOutputLimits(-255, 255);
}
}

void loop(){
#if MOVE_BACK_FORTH
moveBackForth();
#endif

if (!dmpReady) return;                                           
while (!mpuInterrupt && fifoCount < packetSize){          
Serial.print("moveState ");     
Serial.println(moveState);
if(moveState==3){ digitalWrite(mavi,HIGH);digitalWrite(kirmizi,LOW);                                     
setpoint = originalSetpoint - 1;
pid.Compute();
motorController.turnLeft(output, 120);
delay(1);
}
else if(moveState==4){digitalWrite(mavi,LOW);digitalWrite(kirmizi,HIGH);
setpoint = originalSetpoint + 1;
pid.Compute();
motorController.turnRight(output, 120);
delay(1);
}
else if(moveState==0||moveState==1||moveState==2){
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
delay(1);
}

unsigned long currentMillis = millis();
if (currentMillis - time1Hz >= 1000){
loopAt1Hz();
time1Hz = currentMillis;
}
}

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);
input = ypr[1] * 180/M_PI + 180;
}
}                                                     

void loopAt1Hz(){
#if MANUAL_TUNING
setPIDTuningValues();
#endif
}
void moveBackForth(){                                          
if (moveState == 0){digitalWrite(mavi,LOW);digitalWrite(kirmizi,LOW);
setpoint = originalSetpoint;}
else if (moveState == 1){
setpoint = originalSetpoint - movingAngleOffset;}
else if (moveState == 2){
setpoint = originalSetpoint + movingAngleOffset;}
} 

#if MANUAL_TUNING                                             
void setPIDTuningValues(){
pid.SetTunings(kp, ki, kd);
prevKp = kp; prevKi = ki; prevKd = kd;
}
#endif

void serialEvent() {
while (Serial.available()) {
char inChar = Serial.read();

Serial.print("inChar");
Serial.println(inChar);
if (inChar == '#') {
if(id=="Kp"){
kp=value;
}
if(id=="Ki"){
ki=value;
}
if(id=="Kd"){
kd=value;
}
if(id=="AS"){
MIN_ABS_SPEED=value;
}
if(id=="on"){
if(value==1){
wdt_enable(WDTO_15MS);
delay(20);
wdt_reset();
}
else{
digitalWrite(7,LOW);
} }
if(id=="mv"){
int mvs;
mvs=value;
moveState=mvs;
}
if(id=="tn"){
int mvs;
mvs=value;
if(value==1){ }
else{
kp=15;
ki=195;
kd=0.6;
MIN_ABS_SPEED=30;
}}
id = "";
stvalue = "";
qq=0;
break;
}
if(qq<2){
id+=inChar;
}
else{
stvalue+=inChar;
}
value=stvalue.toFloat();
qq++;
}}