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++; }}
Etiketler: andori kontrollü denge robotu, denge, denge robotu, iki teker üzerinde duran robot, uzaktan kumandalı denge
Eklenme Tarihi: 3 Eylül 2021
Konu hakkında yorumunuzu yazın