질문게시판 > Arduino UNO R3 밸런싱 로봇 코딩 관련 질문

TODAY489 TOTAL1,083,138
사이트 이용안내
Login▼/회원가입
최신글보기 질문게시판 기술자료 동영상강좌

아두이노 센서 ATMEGA128 PWM LED 초음파 AVR 블루투스 LCD UART 모터 적외선


BASIC4MCU | 질문게시판 | Arduino UNO R3 밸런싱 로봇 코딩 관련 질문

페이지 정보

작성자 욱2 작성일2020-05-19 19:31 조회57회 댓글1건

본문

	

안녕하십니까!

Arduino Uno R3 밸런싱 로봇을 기반으로 새로운 형식의 밸런싱 로봇을 구현하려고자 합니다.

이를 위해 코딩을 하던 도중 막히는 부분이 있어서 질문 올려봅니다.

 

<코드 1>
 
#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
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

// 아래의 4개의 값을 본인의 로봇에 맞게 튜닝합니다.
/*********파라메터 튜닝 시작*********/
double setpoint= 180;  //로봇이 지면에서 평형을 유지하는 상태의 값입니다.
//다음은 PID 제어기의 Kp, Ki, Kd 파라메타를 설정합니다. 아래의 순서대로 설정합니다.
double Kp = 21;
double Kd = 0.8;
double Ki = 140;
/******파라메터 튜닝 끝*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);


 
volatile bool mpuInterrupt = false;  // MPU6050의 인터럽트 발생유무 확인
void dmpDataReady()
{
 mpuInterrupt = true;
}

void setup() {
 Serial.begin(115200);

 // MPU6050 초기화
 Serial.println(F("Initializing I2C devices..."));
 mpu.initialize();

 // MPU6050 통신확인
 Serial.println(F("Testing device connections..."));
 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

 // DMP 초기화
 devStatus = mpu.dmpInitialize();


 // 기본 옵셋값 설정
 mpu.setXGyroOffset(220);
 mpu.setYGyroOffset(76);
 mpu.setZGyroOffset(-85);
 mpu.setZAccelOffset(1688);

 // 정상동작하는 경우
 if (devStatus == 0)
 {
  // DMP 가동
  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();

  // DMP 사용가능 상태 설정
  Serial.println(F("DMP ready! Waiting for first interrupt..."));
  dmpReady = true;

  // 패킷사이즈 가져오기
  packetSize = mpu.dmpGetFIFOPacketSize();

  //PID 설정
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-255, 255);
 }
 else
 {
  // 오류시
  // 1 = 초기 메모리 에러
  // 2 = DMP 설정 오류
  Serial.print(F("DMP Initialization failed (code "));
  Serial.print(devStatus);
  Serial.println(F(")"));
 }

  //모터 출력핀 초기화
 pinMode (6, OUTPUT);
 pinMode (9, OUTPUT);
 pinMode (10, OUTPUT);
 pinMode (11, OUTPUT);

 //모터 동작 OFF
 analogWrite(6,LOW);
 analogWrite(9,LOW);
 analogWrite(10,LOW);
 analogWrite(11,LOW);
}


void loop() {

 // 오류시 작업중지
 if (!dmpReady) return;
 
 // MPU 인터럽트나 패킷 대기
 while (!mpuInterrupt && fifoCount < packetSize)
 {
  //MPU6050 데이터가 없는 경우 PID 계산
  pid.Compute();

  //시리얼 모니터로 현재 상태 출력
  Serial.print(input); Serial.print(" =>"); Serial.println(output);

  if (input>150 && input<200)
  {
  //로봇이 기울어지는 경우(각도 범위내에서만)

  if (output>0)   //앞으로 기울어지는 경우
  Forward();   //전진
  else if (output<0)  //뒤로 기울어지는 경우
  Reverse();   //후진
  }
  else     //로봇이 기울어지지 않은 경우
  Stop();    //모터 정지

 }

 // 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);   //get value for q
  mpu.dmpGetGravity(&gravity, &q);     //get value for gravity
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  //get value for ypr

  input = ypr[1] * 180/M_PI + 180;
 
 }
}

void Forward()   //전진
{
 analogWrite(6,output);
 analogWrite(9,0);
 analogWrite(10,output);
 analogWrite(11,0);
 Serial.print("F");
}

void Reverse()   //후진
{
 analogWrite(6,0);
 analogWrite(9,output*-1);
 analogWrite(10,0);
 analogWrite(11,output*-1);
 Serial.print("R");
}

void Stop()   //정지
{
 analogWrite(6,0);
 analogWrite(9,0);
 analogWrite(10,0);
 analogWrite(11,0);
 Serial.print("S");
}


<코드 1>은  일반적인 밸런싱 로봇의 코드입니다.
Mpu6050 가속도 센서로부터 입력신호를 받아 pid 제어기에 넣는 구조입니다.

 

<코드 2>

Int sensorpin = 2;
Int ledpin = 9;

Void setup() {
 serial.begin(9600);
}

Void loop() {
 int val = analogRead(sensor);
 serial.println(val)
}

<코드 2>는 압력센서로부터 입력신호를 받아 시리얼 모니터에서 0부터 1023의 값을 확인할 수 있는 코드입니다.
기존 mpu6050 센서의 앞뒤 기울기 신호를 밸런싱로봇 몸체 앞뒤로 달아서 기울기 신호를 압력센서 두개의 오차의 정도로 대체하고자 합니다.

<코드 2>를 <코드 1>에 적용시키고자 하는데 어디서부터 손을 대어야 할지 감이 잡히지 않습니다.

 

 

 

 

 

 

 

 

<코드 1>
 
#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
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

// 아래의 4개의 값을 본인의 로봇에 맞게 튜닝합니다.
/*********파라메터 튜닝 시작*********/
double setpoint= 180;  //로봇이 지면에서 평형을 유지하는 상태의 값입니다.
//다음은 PID 제어기의 Kp, Ki, Kd 파라메타를 설정합니다. 아래의 순서대로 설정합니다.
double Kp = 21;
double Kd = 0.8;
double Ki = 140;
/******파라메터 튜닝 끝*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);


 
volatile bool mpuInterrupt = false;  // MPU6050의 인터럽트 발생유무 확인
void dmpDataReady()
{
 mpuInterrupt = true;
}

void setup() {
 Serial.begin(115200);

 // MPU6050 초기화
 Serial.println(F("Initializing I2C devices..."));
 mpu.initialize();

 // MPU6050 통신확인
 Serial.println(F("Testing device connections..."));
 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

 // DMP 초기화
 devStatus = mpu.dmpInitialize();


 // 기본 옵셋값 설정
 mpu.setXGyroOffset(220);
 mpu.setYGyroOffset(76);
 mpu.setZGyroOffset(-85);
 mpu.setZAccelOffset(1688);

 // 정상동작하는 경우
 if (devStatus == 0)
 {
  // DMP 가동
  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();

  // DMP 사용가능 상태 설정
  Serial.println(F("DMP ready! Waiting for first interrupt..."));
  dmpReady = true;

  // 패킷사이즈 가져오기
  packetSize = mpu.dmpGetFIFOPacketSize();

  //PID 설정
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-255, 255);
 }
 else
 {
  // 오류시
  // 1 = 초기 메모리 에러
  // 2 = DMP 설정 오류
  Serial.print(F("DMP Initialization failed (code "));
  Serial.print(devStatus);
  Serial.println(F(")"));
 }

  //모터 출력핀 초기화
 pinMode (6, OUTPUT);
 pinMode (9, OUTPUT);
 pinMode (10, OUTPUT);
 pinMode (11, OUTPUT);

 //모터 동작 OFF
 analogWrite(6,LOW);
 analogWrite(9,LOW);
 analogWrite(10,LOW);
 analogWrite(11,LOW);
}


void loop() {

 // 오류시 작업중지
 if (!dmpReady) return;
 
 // MPU 인터럽트나 패킷 대기
 while (!mpuInterrupt && fifoCount < packetSize)
 {
  //MPU6050 데이터가 없는 경우 PID 계산
  pid.Compute();

  //시리얼 모니터로 현재 상태 출력
  Serial.print(input); Serial.print(" =>"); Serial.println(output);

  if (input>150 && input<200)
  {
  //로봇이 기울어지는 경우(각도 범위내에서만)

  if (output>0)   //앞으로 기울어지는 경우
  Forward();   //전진
  else if (output<0)  //뒤로 기울어지는 경우
  Reverse();   //후진
  }
  else     //로봇이 기울어지지 않은 경우
  Stop();    //모터 정지

 }

 // 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);   //get value for q
  mpu.dmpGetGravity(&gravity, &q);     //get value for gravity
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  //get value for ypr

  input = ypr[1] * 180/M_PI + 180;
 
 }
}

void Forward()   //전진
{
 analogWrite(6,output);
 analogWrite(9,0);
 analogWrite(10,output);
 analogWrite(11,0);
 Serial.print("F");
}

void Reverse()   //후진
{
 analogWrite(6,0);
 analogWrite(9,output*-1);
 analogWrite(10,0);
 analogWrite(11,output*-1);
 Serial.print("R");
}

void Stop()   //정지
{
 analogWrite(6,0);
 analogWrite(9,0);
 analogWrite(10,0);
 analogWrite(11,0);
 Serial.print("S");
}


<코드 1>은  일반적인 밸런싱 로봇의 코드입니다.
Mpu6050 가속도 센서로부터 입력신호를 받아 pid 제어기에 넣는 구조입니다.

 

<코드 2>

Int sensorpin = 2;
Int ledpin = 9;

Void setup() {
 serial.begin(9600);
}

Void loop() {
 int val = analogRead(sensor);
 serial.println(val)
}

<코드 2>는 압력센서로부터 입력신호를 받아 시리얼 모니터에서 0부터 1023의 값을 확인할 수 있는 코드입니다.
기존 mpu6050 센서의 앞뒤 기울기 신호를 밸런싱로봇 몸체 앞뒤로 달아서 기울기 신호를 압력센서 두개의 오차의 정도로 대체하고자 합니다.

<코드 2>를 <코드 1>에 적용시키고자 하는데 어디서부터 손을 대어야 할지 감이 잡히지 않습니다. 

  • BASIC4MCU 작성글 SNS에 공유하기
  • 페이스북으로 보내기
  • 트위터로 보내기
  • 구글플러스로 보내기

댓글 1

조회수 57

master님의 댓글

master 작성일

무슨 뜻인지 이해를 하지 못하겠습니다.
복잡한 기능의 소스코드 및 직접 구동하지 않으면 코드를 작성하기 어려운 경우에는 답변이 불가능하기도 합니다.

질문게시판HOME > 질문게시판 목록

MCU, AVR, 아두이노 등 전자공학에 관련된 질문을 무료회원가입 후 작성해주시면 전문가가 답변해드립니다.
ATMEGA128PWMLED초음파
아두이노AVR블루투스LCD
UART모터적외선ATMEGA
전체 스위치 센서
질문게시판 목록
제목 작성자 작성일 조회
공지 MCU, AVR, 아두이노 등 전자공학에 관련된 질문은 질문게시판에서만 작성 가능합니다. 스태프 19-01-15 1698
공지 사이트 이용 안내댓글[11] master 17-10-29 11198
공지 [무료 공개] 소스코드 하이라이트 v2.0 beta [2013.02.07]댓글[1] 이미지첨부파일 master 18-01-23 4215
질문 아두이노 7세그먼트댓글[1] 새글 Cordingplz 20-06-02 8
질문 atmega128 uart0사용법댓글[6] 새글 jiseok 20-06-02 14
질문 아두이노 우노 외부전원 9v댓글[2] 새글 helpme 20-06-02 9
질문 솔레노이드 벨브를 이용한 가스벨브제어댓글[2] 새글 밀키스 20-06-02 11
질문 아두이노 모터 센서 오류댓글[1] 새글 박지성룡 20-06-02 14
질문 아두이노 인터럽트 led 새글 옹아 20-06-02 19
답변 답변글 답변 : 아두이노 인터럽트 led 새글 master 20-06-02 9
질문 avr 블루투스, 초음파, LED 설계댓글[1] 새글 전과자14학번 20-06-02 19
답변 답변글 답변 : avr 블루투스, 초음파, LED 설계 새글 master 20-06-02 12
질문 진동센서값 오류잡아주세요,,,댓글[1] 새글 팬다 20-06-02 12
질문 공부방향 문의댓글[1] 새글 갈비야 20-06-02 12
질문 질문 하나 드리겠습니다... 제발 도와주십시오 제발... ㅠㅠ댓글[1] 이미지새글 아두이노어려워 20-06-01 27
질문 타이머/카운터 1을 10비트 분해능의 Fast PWM 모드로 설정, 문제좀 알려주세요댓글[1] 새글 Tani 20-06-01 18
질문 AVR 코드로 7세그먼트 4자리 불 들어오는법이요!!댓글[1] 이미지새글첨부파일 dslaks 20-06-01 21
질문 아두이노 시리얼 통신 새글 눈의궁전 20-06-01 17
답변 답변글 답변 : 아두이노 시리얼 통신 새글 master 20-06-01 18
질문 아두이노 소스코드 합치기 질문 드립니다.댓글[2] 새글 하기싫다 20-06-01 25
질문 키패드설정값 문자표기 릴레이작동댓글[2] 왕왕초a 20-06-01 19
질문 아트메가와 아두이노 통신 하는방법! jiseok 20-06-01 20
답변 답변글 답변 : 아트메가와 아두이노 통신 하는방법!댓글[3] master 20-06-01 17
질문 atmega128을 이용하여 PORTA에 2진수 패턴이 출력되도록 하는 C프로그램 작성 혀스석 20-05-31 30
답변 답변글 답변 : atmega128을 이용하여 PORTA에 2진수 패턴이 출력되도록 하는 C프로그램 작성댓글[2] master 20-06-01 24
질문 센서 평균값 구하기댓글[2] 팬다 20-05-31 26
답변 답변글 답변 : 센서 평균값 구하기 master 20-06-01 9
질문 Atmega128P BLDC 모터관련 질문좀할게요(3)댓글[1] 차니 20-05-30 38
답변 답변글 답변 : Atmega128P BLDC 모터관련 질문좀할게요(3) master 20-05-30 36
질문 (질문)atmega128을 이용한 LED제어댓글[2] 발양말 20-05-29 68
게시물 검색

2020년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2019년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2018년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
Privacy Policy
MCU BASIC ⓒ 2020
모바일버전으로보기