질문게시판 > PID제어 코드 관련 2

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

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


BASIC4MCU | 질문게시판 | PID제어 코드 관련 2

페이지 정보

작성자 서씨 작성일2018-05-05 15:41 조회12,984회 댓글4건

본문

	

디지털로 다 바꿨는데도 여전히 안되네요. 어떻게 해야할까요? 아니면 혹시 아날로그로 다 바꿀 수 있는 방법은 없나요? m12랑 m22가 여전히 안되네요.

#include "Wire.h"
#define IR_TRIG 9
#define IR_ECHO 8

union {int16_t val; byte med[sizeof(val)];} dat; //union 사용.

const byte m11=10; const byte m12=12; const byte m21=11; const byte m22=13;
const byte DT=5; const float pi=3.141592;
int16_t accX, accY, accZ, gyroX, gyroY, gyroZ;
uint32_t timer; uint8_t i2cData[14]; float  gyroYangle, compAngleY, gyroYrate;


float A=0.96, croll0=0, croll, roll;
const float scaling=0.0076336; // MPU6050: +/-250degree per 16bit (500/65536)
uint32_t rt0=0, oldt3; int16_t Output33;
float inc=50.0, bangle=0.0; //inc: angle band in control, bangle: balancing angle to be maintained

float Input3, Output3, Output31, olderror3=0, oldintegral3=0;
float dt=(DT)/1000.0, fc=120.0 ; //filtered data, sampling time, cutoff freq.
float alpha=dt/((1/(2*pi*fc))+dt); //LPF for the gyrosensor
float fc1=4.0; //Cutoff frequency [Hz] for the derivative gain
float alpha1=dt/((1/(2*pi*fc1))+dt); //LPF the derivative gain
int16_t yaw, rotation=150;
const float runangle=18.0; //forward/backward reference angle to be tracked

// Very Important!! Control Gain
float Kpg =90, Kig = 20, Kdg =0.1; //PID gain: Gyroscopic sensor at 12v
//

void Receive() //Receiving the refrence input from the tranmitter via the Bluetooth
{while(Serial2.available()>=((2*sizeof(dat.med))+1))
  {if (Serial2.read()=='@'){
  for (int16_t i=0; i<sizeof(dat.med); i++) {dat.med[i]=Serial2.read();}
  bangle= -(((float) dat.val)/500.0)*runangle; Serial.println(bangle); //-5 degree~ 5 degree
  for (int16_t i=0; i<sizeof(dat.med); i++) {dat.med[i]=Serial2.read();}
  yaw=map(dat.val, -500, 500, rotation, -rotation); Serial.println(yaw); }
  else {}}}

void motion() //Move the balancing machine upon request from the arduino
{ if (abs(Input3)<inc){
Output3=PID3(bangle-0.0,-Input3);
Output31=constrain(Output3, -255,255);
int16_t Output311=Output31;
int motorA=Output311+yaw, motorB=Output311-yaw;
if(motorA>0) {analogWrite(m11,min(motorA,200));analogWrite(m12,min(motorA,200));
  if(motorB>0) {analogWrite(m21,min(motorB,200));analogWrite(m22,0);}
  else {analogWrite(m21,min(abs(motorB),200));analogWrite(m22,min(abs(motorB),200));}}
else {analogWrite(m11,min(abs(motorA),200)); analogWrite(m12,0);
  if(motorB>0) {analogWrite(m21,min(motorB,200));analogWrite(m22,0);}
  else {analogWrite(m21,min(abs(motorB),200));analogWrite(m22,min(abs(motorB),200));}}}
else {analogWrite(m11,0);analogWrite(m21,0);}

}


float gyroangle(){
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);  accY = ((i2cData[2] << 8) | i2cData[3]);  accZ = ((i2cData[4] << 8) | i2cData[5]); 
gyroX = (i2cData[8] << 8) | i2cData[9];  gyroY = (i2cData[10] << 8) | i2cData[11];  gyroZ = (i2cData[12] << 8) | i2cData[13];
float dt = (float)(micros() - timer) / 1000000.0;
timer = micros();
float aroll=atan2(accY,accZ)*180.0/pi; //in degree, -180~180
float groll=scaling*(gyroX*dt)+croll0;
croll=A*groll+(1-A)*aroll; //Complementary filter, cf) float K = 0.8; float A = K / (K + dt);
croll0=croll;
roll=(1-alpha)*roll+alpha*croll;
float y=(1-alpha)*y+alpha*roll;
//Serial.println(y);
return y;}

float PID3(float Setpoint, float Input) //PID Control w.r.t. Gyro sensor
{float newerror3=Setpoint-Input; //
uint32_t newt3=millis();
uint32_t dt3=newt3-oldt3; //in milliseconds
float Kpgg,Kigg,Kdgg;
if(newerror3*oldintegral3<0){oldintegral3=0.0; Kpgg=1*Kpg; Kigg=2*Kig; Kdgg=1*Kdg;} //Gain Weighting
else {Kpgg=Kpg; Kigg=Kig; Kdgg=Kdg;}
float newintegral3=oldintegral3+newerror3*dt3/1000;
float Kpgg0=Kpgg*newerror3;
float Kpgg1=constrain(Kpgg0,-350.0,350.0);
float newerror33=(1-alpha1)*newerror33+alpha1*newerror3; //LPF for the D term
float derivative3=((newerror33-olderror3)/dt3)*1000;
float derivative330=Kdgg*derivative3;
float derivative33=constrain(derivative330,-350.0,350.0);
float iterm=Kigg*newintegral3;
float iterm3=constrain(iterm,-330.0,330.0);
float Output3=Kpgg1+iterm3+derivative33;
olderror3=newerror3; oldintegral3=newintegral3; oldt3=newt3;
//Serial2.print(Kpgg1);Serial2.print("\t");Serial2.print(iterm3);Serial2.print("\t");Serial2.println(derivative33);
return Output3;}


void setup() {
pinMode(m12,1); pinMode(m22,1);
Wire.begin();  while (i2cWrite(0x6B, 0x01, true)); // PLL(phase locked loop)출력주파수 일정하게 유지 with X axis gyroscope reference and disable sleep mode
Serial.begin(9600); Serial2.begin(9600);Serial2.flush();
pinMode(m11, OUTPUT);
pinMode(m21, OUTPUT);
pinMode(m12, OUTPUT);
pinMode(m22, OUTPUT); 
 
  //초음파 센서 핀모드 설정
  pinMode(IR_TRIG, OUTPUT);
  pinMode(IR_ECHO, INPUT);

}

void loop() {
    float duration, distance;
  digitalWrite(IR_TRIG, HIGH);
  delay(10);
  digitalWrite(IR_TRIG, LOW);
 
  // echoPin 이 HIGH를 유지한 시간
  duration = pulseIn(IR_ECHO, HIGH);
  // HIGH 였을 때 시간(초음파가 보냈다가 다시 들어온 시간)을 가지고 거리를 계산 한다.
  // 340은 초당 초음파(소리)의 속도, 10000은 밀리세컨드를 세컨드로, 왕복거리이므로 2로 나눠주면 거리가 cm 로 나옴
  distance = ((float)(340 * duration) / 10000) / 2;
  Serial.print("\nDIstance : ");
  Serial.println(distance);
    
  if(distance < 20) {         //장애물 감지 (20cm 이내)
    back();
   
           int rnd = random(0,2);   //장애물 감지시 좌/우회전 랜덤처리
     Serial.println(rnd);
     if(rnd == 0){
        Serial.println("right");
        right();
     }else{
        Serial.println("left");
        left();
     }
   }

  Receive(); Input3=gyroangle(); motion(); delay(DT);

}

void left(){
  digitalWrite(m11, LOW); //오른쪽 뒤
  digitalWrite(m12,HIGH);
 digitalWrite(m21,LOW);
 digitalWrite(m22,HIGH);
  delay(3000);
}
void right(){
  digitalWrite(m11, HIGH);
  digitalWrite(m12, LOW);
  digitalWrite(m21, HIGH); //왼쪽 앞
  digitalWrite(m22, LOW);
  delay(3000);
}
void back(){
  digitalWrite(m11, HIGH);
  digitalWrite(m12, LOW);
  digitalWrite(m21, LOW);
  digitalWrite(m22, HIGH);
  delay(3000);
}

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

댓글 4

조회수 12,984

master님의 댓글

master 작성일

analogWrite()가 아직도 많으니 모두 바꾸세요

서씨님의 댓글

서씨 작성일

pid제어하는데 다 디지털로 바꿔버리면 제어가 어렵지 않나요 ??? 아날로그로 바꾸는 방법은 없나요?

서씨님의 댓글

서씨 작성일

EN핀을 하시라고 하셨는데 , 그럼 어떻게 해야할까요.... 도움이 될 만한 자료가 있을까요?

master님의 댓글

master 댓글의 댓글 작성일

회로도를 올리고
모터와 드라이버모듈 자료도 올려야합니다.
뭘 사용하고 있는지 제가 알 길이 없는데
어떻게 도와드리겠습니까...

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

MCU, AVR, 아두이노 등 전자공학에 관련된 질문을 무료회원가입 후 작성해주시면 전문가가 답변해드립니다.
ATMEGA128PWMLED초음파
아두이노AVR블루투스LCD
UART모터적외선ATMEGA
전체 스위치 센서
질문게시판 목록
제목 작성자 작성일 조회
질문 안녕하세요! 질문드립니다댓글[1] 비공개 18-05-18 7328
질문 변수에 문자를 대입할때 여러 문자를 넣을 방법이 없나요?? hdki 18-05-17 6200
답변 답변글 답변 : 변수에 문자를 대입할때 여러 문자를 넣을 방법이 없나요??댓글[1] master 18-05-17 14432
질문 코드비전으로 변환댓글[1] 박희범 18-05-17 7690
질문 아두이노 펄스출력 DrVoid 18-05-17 8370
답변 답변글 답변 : 아두이노 펄스출력댓글[1] master 18-05-17 6864
질문 atmega128 포트D댓글[1] c린이 18-05-16 7648
질문 아트메가 통신댓글[3] 빠박이빠박 18-05-16 10081
질문 PWM질문입니다. 첨부파일 DrVoid 18-05-16 6330
답변 답변글 답변 : PWM질문입니다.댓글[1] master 18-05-16 6976
질문 안녕하세요. Lora 통신 관련해서 질문 드렸었는데, 재질문드립니다.댓글[7] 비공개 18-05-14 19449
질문 오늘 로드셀 관련하여 지식인에 질문드렸던사람입니다댓글[1] zxcv1vcxz 18-05-14 8366
질문 아두이노 우노에 적외선센서 2개 연결하는법댓글[1] 이미지첨부파일 cho11 18-05-14 16872
질문 아두이노관련 질문드립니다.댓글[1] 최승아 18-05-14 7698
질문 적외선센서와 mp3모듈 연결 소스코드댓글[1] dm501 18-05-14 7922
답변 답변글 답변 : 적외선 거리센서 공식 master 18-05-15 7078
질문 아두이노 적외선센서와 mp3플레이어 모듈 연결댓글[1] dm501 18-05-14 10124
질문 아두이노 어댑터를 뺏다가 연결했을떄 이더넷쉴드가 연결이 안됩니다댓글[1] 이미지첨부파일 cho11 18-05-14 9918
질문 atmega128 장애물회피자동차 스텝모터댓글[1] alsdn6431 18-05-14 9295
질문 아두이노 센서 2개 사용 코드 관련 질문 kimhk 18-05-14 10536
답변 답변글 답변 : fade (target 과 soundPre 의 관계를 이해) master 18-05-14 6617
답변 답변글 답변 2: 사운드 센서 신호 검출 master 18-05-14 8002
답변 답변글 답변 3: 복수개의 LED PWM 구동 방법 master 18-05-14 6434
질문 아두이노 카메라에 대해 질문있습니다.댓글[1] 후후훙 18-05-13 8789
질문 아두이노 센서 연결 질문댓글[3] dsfkmfkds 18-05-13 13360
질문 아두이노 스톱워치 관련입니다.댓글[1] 비공개 18-05-13 8678
질문 아두이노 로봇청소기를 제작중입니다내용> 안녕하세요 아두이노 메가를 이용한 로봇청소기를 구현중에 있습니다. …댓글[1] 동수킴 18-05-12 9652
답변 답변글 답변 : 아두이노 로봇청소기를 제작중입니다내용> 안녕하세요 아두이노 메가를 이용한 로봇청소기를 구현중에 있습… master 18-05-13 7834
질문 ATmega128 led 하나씩 시프트 하는거에 대해 비공개 18-05-12 7657
답변 답변글 답변 : ATmega128 led 하나씩 시프트 - 반복문 master 18-05-12 8145
게시물 검색

2022년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2021년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
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
모바일버전으로보기