الروبوت ذاتي التوازن:
باستخدام لوحة Android ، يمكنك التحكم في السرعة والاتجاه وحتى توازن الروبوت ذي العجلتين فقط. عادتاً ما يكون الروبوت ذو العجلتين ذاتي التوازن طريقة رائعة لفهم التوازن. فهي سريعة وفعالة وممتعة في العمل عليها. ولكن ماذا لو كان بإمكانك أيضًا التحكم في روبوت ذاتي التوازن ذي العجلتين باستخدام لوحة Android؟
يعد الروبوت ذو العجلتين ذاتي التوازن طريقة رائعة للالتفاف. إنها ممتعة وفعالة ويسهل التحكم فيها باستخدام Android. في هذه المقالة ، سنوضح لك كيفية استخدام متحكم Android للتحكم في روبوت ذاتي التوازن بعجلتين.
الروبوت ذاتي التوازن:
يشبه الروبوت ذاتي التوازن إلى حد ما البندول المقلوب. على عكس البندول التقليدي، يتأرجح ذهابًا وإيابًا بعد أن يبدأ من دفعة ، وذلك بفضل المستشعرات التي تكتشف الحركة والمحركات التي تساعده على تصحيح نفسه. ، سيعود الروبوت ذو التوازن الذاتي بسرعة إلى وضعه المستقيم. هذا لأن الروبوت ذاتي التوازن يحتوي على مستشعرات تكتشف عندما يكون غير متوازن ومحركات تساعده على استعادة توازنه.
البندول المقلوب هو البندول الذي يكون مركز كتلته فوق نقطة ارتكازه. إنه غير مستقر وسيسقط ما لم يتحرك باستمرار. وأشهر بندول مقلوب هو جسم الإنسان. نحافظ على استقامة أنفسنا من خلال إجراء تعديلات صغيرة باستمرار على مركز كتلتنا. إذا توقفنا عن الحركة ، فسوف نسقط.
والقدير بالذكر حساسات التوازن يتم استخدامها بكثره في بعض أنواع الروبوتات.
أساسيات الروبوت التوازن الذاتي:
الروبوت ذو التوازن الذاتي هو روبوت يمكنه البقاء مستقيماً أثناء الحركة بفضل خوارزمية تأخذ البيانات من أجهزة الاستشعار(حساسات) التي تقيس ميل الروبوت. ثم تستخدم الخوارزمية هذه المعلومات لحساب مقدار القوة التي يجب تطبيقها على عجلات الروبوت من أجل الحفاظ على توازنها.
للحفاظ على توازن الروبوت ، تحتاج إلى تدوير العجلات في اتجاه سقوطه. للقيام بذلك ، تقوم بضبط زاوية الميل والسرعة التي تسقط بها. يوفر لنا مستشعر MPU6050 هذه المعلومات حتى نتمكن من إرسال إشارات التحكم المناسبة إلى سائقي المحركات.
بعد معالجة البيانات من المستشعر ، يتم إرسال إشارات التحكم المناسبة إلى مشغل المحركات للحفاظ على التوازن.
قياس زاوية الميل باستعمال حساس جيروسكوب:
من خلال MP06050 يمكن قياس السرعة الدورانية على طول المحاور(x-y-z). بالنسبة لروبوت المتوازن المستخدم يكفي قياس سرعة الدوران على محور(x) فقط للحصول على معدل سقوط .من خلال الكود نقوم بتحول سرعة الدوران على المحور x الى درجة على الثانية ثم نقوم بضربه في الزمان (loopTime).
نظرية التحكم:
لتحقيق التوازن الروبوت ، يتم استخدام سلسلة PID. إنه في الأساس وحدتا تحكم PID متصلتان. وحدة تحكم PID واحدة (حلقة التحكم الداخلية) تأخذ الفرق بين الزاوية المطلوبة والزاوية الحالية وتحسب سرعة المحركات. تأخذ وحدة تحكم PID الثانية (حلقة التحكم الخارجية) الفرق بين سرعة المحرك وسرعة نقطة الضبط كإدخال وتحسب الزاوية المطلوبة لعملية التوازن.
يسمح نظام التحكم هذا للإنسان الآلي بالعودة إلى موضعه الأصلي عند الإزعاج. يستطيع الروبوت أيضًا العثور على زاوية ضبط جديدة للموازنة إذا تم تحريك مركز الجاذبية ، أو إذا كان الروبوت يقف على مستوى مائل. نظرًا لأنني أستخدم محركات السائر ، فليس هناك حاجة إلى تشفير عجلة للتغذية المرتدة. ستعمل المحركات دائمًا بالسرعة المطلوبة.
على سبيل المثال: إذا أردنا أن يقف الروبوت ثابتًا ، فيمكننا ضبط سرعة نقطة الضبط على 0. إذا بدأ الروبوت في الانجراف للأمام (إذا كان الروبوت ثقيلًا في الأمام ، أو إذا تم دفعه) ، فإن الجزء الأول من عنصر التحكم الخارجي سوف تقوم الحلقة بضبط زاوية نقطة الضبط للانحناء للخلف لمحاولة الحصول على السرعة إلى 0. هذا يعمل لأن تكامل السرعة هو الموضع. ستكون زاوية الميل الخلفي متناسبة مع المسافة من نقطة البداية. يجعل الجزء P من حلقة التحكم الخارجية الروبوت يميل دائمًا إلى الخلف قليلاً بالنسبة لسرعة سيره ، وهذا يخفف من الاهتزازات التي يسببها الجزء I ، وبدون الجزء P ، فإن الروبوت سوف يتحرك ذهابًا وإيابًا إلى أجل غير مسمى ، وليس أبدًا توقف. لا حاجة لجزء D في حلقة التحكم الخارجية.
عند ضبط هذا النوع من النظام ، أبدأ بحلقة التحكم الداخلية التي تحسب سرعة المحرك بناءً على الزاوية. قمت بتعيين زاوية نقطة الضبط على 0 (بشكل مستقيم) ومكاسب I و D إلى 0. ثم قم بزيادة كسب P حتى أحصل على التذبذبات. ثم أقوم بتقليل مكاسب P بشكل طفيف وأضف بعض مكاسب I و D. عندما يتم ذلك بشكل صحيح ، يجب أن يكون الروبوت قادرًا على التوازن ، لكنه سينجرف بعيدًا من تلقاء نفسه ، أو عند الدفع. يجب أن يعود الروبوت إلى زاويته المستقيمة بسرعة نسبيًا عند دفعه ، ويجب ألا يكون هناك أي اهتزازات أو اهتزازات عالية السرعة أو تجاوزات كبيرة. بعد ذلك ، يمكن ضبط حلقة التحكم الخارجية في زاوية الضبط بناءً على السرعة. عادة ما يكون هذا أسهل بكثير ، ما عليك سوى إضافة بعض مكاسب P و I وتجربتها.
بناء الروبوت:
فيما يلي صورة البناء الأساسية لروبوت التوازن الذاتي
عناصر مشروع الروبوت ذاتي التوازن:
- Arduino
- مستشعر جيروسكوب (MP06050)
- Bluetooth-05-HC
- 2Moto DC او Stepping motor
- Battery
- Drive motors
- Connecting wires
1. مخطط الدائرة لمشروع الروبوت ذاتي التوازن:
كود مشروع الأول:
//simple A4988 connection
//jumper reset and sleep together
//connect VDD to Arduino 3.3v or 5v
//connect GND to Arduino GND (GND near VDD)
//connect 1A and 1B to stepper coil 1
//connect 2A and 2B to stepper coil 2
//connect VMOT to power source (9v battery + term)
//connect GRD to power source (9v battery - term)
/*
Motor driver pins:
- D5 - STEP1 (PORTD 5)
- D6 - STEP2 (PORTD 6)
- D7 - DIR1 (PORTD 7)
- D8 - DIR2 (PORTB 0)
- D4 - ENABLE (for both)
*/
#define LED 13
#define STEP1 5
#define STEP2 6
#define DIR1 7
#define DIR2 8
#define ENABLE 4
#define DELAY 100
#define STEPO 16 // 1, 2, 4, 8, 16
#define STEPS 400*STEPO
#define STEPS2 200*STEPO
int a1, a2; // separate if used simultaneously
//#define ESPERA delay(DELAY)
#define ESPERA delayMicroseconds(DELAY)
void setup()
{
pinMode(LED, OUTPUT);
pinMode(STEP1, OUTPUT);
pinMode(STEP2, OUTPUT);
pinMode(DIR1, OUTPUT);
pinMode(DIR2, OUTPUT);
pinMode(ENABLE, OUTPUT);
digitalWrite(LED, LOW);
digitalWrite(STEP1, LOW);
digitalWrite(STEP2, LOW);
digitalWrite(DIR1, LOW);
digitalWrite(DIR2, LOW);
digitalWrite(ENABLE, LOW);
}
void stepa1(void)
{
digitalWrite(STEP1, HIGH);
ESPERA;
digitalWrite(STEP1, LOW);
ESPERA;
}
void stepa2(void)
{
digitalWrite(STEP2, HIGH);
ESPERA;
digitalWrite(STEP2, LOW);
ESPERA;
}
void loop()
{
a1 = 0;
while(a1<STEPS)
{
if (a1==0)
{
digitalWrite(LED, LOW);
digitalWrite(DIR1, LOW);
}
else
{
if(a1==STEPS2)
{
digitalWrite(LED, HIGH);
digitalWrite(DIR1, HIGH);
}
}
stepa1();
a1++;
}
a2 = 0;
while(a2<STEPS)
{
if (a2==0)
{
digitalWrite(LED, LOW);
digitalWrite(DIR2, LOW);
}
else
{
if(a2==STEPS2)
{
digitalWrite(LED, HIGH);
digitalWrite(DIR2, HIGH);
}
}
stepa2();
a2++;
}
}
#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 = ""; // a string to hold incoming data
String stvalue = "";
double value;
int qq=0;
MPU6050 mpu; // MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container, orientation/motion vars
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
double kp=15 , ki=190, kd=0.6; //PID
double prevKp, prevKi, prevKd;
double originalSetpoint = 177;
double setpoint = originalSetpoint;
double movingAngleOffset = 3.3;
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth
#if MANUAL_TUNING
PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
PID pid(&input, &output, &setpoint, 15, 195, 0.6, DIRECT);
#endif
//MOTOR CONTROLLER
int ENA = 6;
int IN1 = 11;
int IN2 = 12;
int IN3 = 10;
int IN4 = 9;
int ENB = 5;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1,1);
//timers
long time1Hz = 0;
long time5Hz = 0;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady(){
mpuInterrupt = true;
}
void setup(){
wdt_disable();
pinMode(7,OUTPUT); //pin 7 on-off (motor enable)
digitalWrite(7,HIGH);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE // join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
mpu.initialize();
devStatus = mpu.dmpInitialize(); // supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus == 0){ // make sure it worked (returns 0 if so)
mpu.setDMPEnabled(true); // turn on the DMP, now that it's ready
attachInterrupt(0, dmpDataReady, RISING); // enable Arduino interrupt detection
mpuIntStatus = mpu.getIntStatus();
dmpReady = true; // set our DMP Ready flag so the main loop() function knows it's okay to use it
packetSize = mpu.dmpGetFIFOPacketSize(); // get expected DMP packet size for later comparison
pid.SetMode(AUTOMATIC); //setup PID
pid.SetSampleTime(5);
pid.SetOutputLimits(-255, 255);
}
}
void loop(){
#if MOVE_BACK_FORTH
moveBackForth();
#endif
if (!dmpReady) return; // if programming failed, don't try to do anything
while (!mpuInterrupt && fifoCount < packetSize){ // wait for MPU interrupt or extra packet(s) available,no mpu data - performing PID calculations and output to motors
if(moveState==3){ //right side
setpoint = originalSetpoint - 1;
pid.Compute();
motorController.turnLeft(output, 120);
delay(1);
}
else if(moveState==4){ // left side
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; // reset interrupt flag and get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount(); // get current FIFO count
if ((mpuIntStatus & 0x10) || fifoCount == 1024){ // check for overflow (this should never happen unless our code is too inefficient)
mpu.resetFIFO(); // reset so we can continue cleanly
} // otherwise, check for DMP data ready interrupt (this should happen frequently)
else if (mpuIntStatus & 0x02){
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // wait for correct available data length, should be a VERY short wait
mpu.getFIFOBytes(fifoBuffer, packetSize); // read a packet from FIFO, track FIFO count here in case there is > 1 packet available
fifoCount -= packetSize; // (this lets us immediately read more without waiting for an interrupt)
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
} // end of the loop
void loopAt1Hz(){
#if MANUAL_TUNING
setPIDTuningValues();
#endif
}
void moveBackForth(){ //move back and forth
if (moveState == 0)
setpoint = originalSetpoint;
else if (moveState == 1)
setpoint = originalSetpoint - movingAngleOffset;
else if (moveState == 2)
setpoint = originalSetpoint + movingAngleOffset;
}
#if MANUAL_TUNING //PID Tuning (3 potentiometers)
void setPIDTuningValues(){
pid.SetTunings(kp, ki, kd);
prevKp = kp; prevKi = ki; prevKd = kd;
}
#endif
/*
SerialEvent occurs whenever a new data comes in the
hardware serial RX. This routine is run between each
time loop() runs, so using delay inside loop can delay
response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
char inChar = Serial.read();
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++;
}}
فكره رائعه جدا يعني
ردحذفربنا يزيدك يارب♥️
ردحذفمشروع رائع
ردحذف