프로토타이핑 - 34. 밸런싱 로봇 예제 살펴보기

일단 mpu6050 의 dmp 기능으로 roll, pitch, yaw 출력되는 예제를 돌려보고

 

이번에는 밸런싱 로봇 예제를 살펴보았다.

 

ref : https://maker.pro/arduino/projects/build-arduino-self-balancing-robot

 

 

 

 

 

 

 

이 예제에서는 아두이노 나노를 기반으로 L298N 모터 드라이버와 mpu6050 등이 연결되어잇다.

 

 

잠시 L298N 모터 드라이버를 다시 살펴보면

 

 

다음 링크에서 L298N 모터 드라이버의 핀에 대해서 잘 설명해주고 있다.

 

ref :https://m.blog.naver.com/PostView.nhn?blogId=eduino&logNo=221030701469&proxyReferer=https:%2F%2Fwww.google.com%2F

 

 

나노(나는 우노보드를 사용할거지만)와 연결하는 핀이 ENA/ENB, IN1, IN2, IN3, IN4가 있는데

 

ENA와 ENB는 PWM 신호로 각 모터의 속도 제어용

 

중간에 4개의 IN1 ~ 4는 방향 지정용이라 한다.

 

 

 

 

모터 드라이버는 잠깐 나두고

 

일단 아두이노 소스를 보면 다음과 같다.

 

 

 

PID, 모터 제어, MPU6050 내용들이 섞여있다보니 다소 길어보인다.

 

 

#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 MIN_ABS_SPEED 20

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

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

//PID
double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 50;   
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}


void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#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

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); // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)
if (devStatus == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();

//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255); 
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}


void loop()
{
// if programming failed, don't try to do anything
if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize)
{
//no mpu data - performing PID calculations and output to motors 
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);

}

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}

 

 

 

처음에는 사용하는 라이브러리들 인클루드, 정의할것들 정의해주고

 

 

 

 

 

 

 

mpu 6050 관련 변수 선언

 

 

쿼터니언, 오일러각, 중력 벡터 변수 선언

 

 

 

PID 관련 변수들도 선언하여

pid 인스턴스 생성

 

 

 

 

모터 제어기 변수 선언,

인스턴스도 초기화

타이머와 인터럽트 관련 내용

 

 

 

이제 셋업문으로 들어와보자

 

"I2Cdev.h" 내부를 보면 I2CDEV_IMPLEMENTATION이 정의되어 있으므로

 

Wire.begin()과 TWBR = 24;이 수행된다.

 

이후 시리얼 통신 시작

 

 

 

 

이후는 mpu6050 초기화과정

 

이전에 봤던 mpu6050 예제와 동일

 

 

 

 

 

mpu6050이 잘 초기화되면

 

DMP 사용할 준비와

pid 변수들이 설정된다.

 

초기화 실패시 에러 출력

 

 

 

135 : dmp 데이터가 준비안되면 시간이 조금 지난뒤 다시수행

 

138 ~ 145 : mpu 인터럽트가 발생안하고 fifo데이터가 패킷 사이즈보다 작은 경우

        => PID 계산, 모터 이동

 

나머지는 인터럽트 플래그 리셋, 인터럽트 상태와 fifocount 취득

 

 

오버플로 체크

- 피포 데이터가 너무 쌓이거나 인터럽트 상태가 0x10인 경우 mpu 리셋

 

 

dmp 데이터가 준비되면,

1. fifo 관련 읽고, 카운팅 루틴

2. fifo 데이터로 쿼터니언 계산 -> 중력 가속도 계산 -> 오일러각 계산

3. 오일러각 출력+

 

 

 

 

 

대강 코드는 이정도로 정리하고

 

Pid와 LmotorController는 조금봐야될거같다.

 

 

일단 PID부터

 

 

 

 

 

 

PID  헤더는 대충 pass

 

 

 

 

 

PID 클래스

 

pid 게인, 제어 방향, 시간, 포인터 변수 등을 지정

 

 

 

 

1. now 처음 시간 취득

2. timechange : 시간 변화 취득

3. 샘플링 타임보다 같거나 크면 -> 시간이 충분히 지나면 pid 로직 수행

4. ino 파일에서 전달받은 myInput을 input으로 초기화

* myInput은 ypr에서 피치각

5. setPoint - input으로 오차 구함. setPoint는 균형 상태에서의 피치각

6. 적분항 Iterm, 미분항 DTerm 계산

7. pid 출력 계산 

 

 

 

ino 파일에서 pid.compute()를 호출하여 pid 출력이 계산되면

 

이대로 모터 컨트롤러가 이동시킨다.

 

 

 

 

우선 모터 컨트롤러 클래스

 

우선 모터 A, B 상수와 핀 번호들이 지정된다.

 

 

 

 

ino 파일에서 호출한 move 함수

 

pid의 output이 여기서는 speed로 사용된다.

 

속도가 음수면 -1 방향으로 

 

음수 시

 speed가 minAbsSpeed보다 크면 minAbsSpeed

 speed가 -255보다 크면 speed 사용

 => - minAbsSpeed(= -20) > speed  > -255

  -> speed가 -20보다 크면 -20 사용

  -> speed가 -255보다 작으면 -255사용

 

속도의 범위 20 ~ 255, -20 ~ -255

 

 

ena, enb로 realSpeed * 모터 상수로 pwm 신호가 전달

 

 

 

대강 소스 흐름은 이정도만 보면 될거같다.

 

구현이 문제지만..