아두이노 2 wheel balancing robot 으로 제어기를 짜보려고 하는데요.
C언어가 처음이라 아무것도 모르겠네요...
코드 해석좀 부탁드려도 될까요?
/*
* TWO WHEELER BALANCING
*
* Need to have "I2C dev" folder in Arduino libraries
* Need to have "MPU6050" folder in Arduino libraries
* Robot will try and balance itself using the MPU6050 sensor.
*
* Best used on OSEPP Mechanical Kits.
* 2WBAL-01
*
* created 08 Nov 2016
* by Sui Wei
*
*/
//include motor code
#include "TBMotor.h"
OseppTBMotor Motor1(12, 11);
OseppTBMotor Motor2(8, 3);
//OseppTBMotor Motor3(7, 6);
//OseppTBMotor Motor4(4, 5);
void setup() {
Serial.begin(115200);
updateIMU();
//Change the timer frequency
//To avoid the frequency of hearing.
TCCR2B &= ~7;
TCCR2B |= 1;
}
const float offset_a = 2;
int leftSpeed;
int rightSpeed;
float ctrlTurn = 0;
float ctrlwalk = 0;
extern float angle;
float PID_Balance(float e, float kp, float ki, float kd)
{
static float es = 0, sum = 0;
float r;
sum += e;
if(sum>180)sum=180;else if(sum<-180)sum=-180;
r = kp * e + ki * sum + kd * (e - es);
es = e;
return r;
}
void PIDSetSpeed()
{
leftSpeed = rightSpeed = PID_Balance(angle+ctrlwalk, 20, 1, 60);
leftSpeed+=ctrlTurn;
rightSpeed-=ctrlTurn;
ctrlTurn *= 0.95;
ctrlwalk *= 0.9;
if (leftSpeed > 255)leftSpeed = 255; else if (leftSpeed < -255)leftSpeed = -255;
if (rightSpeed > 255)rightSpeed = 255; else if (rightSpeed < -255)rightSpeed = -255;
}
void loop() {
ProcessCommand();
if(!updateIMU())return;
PIDSetSpeed();
if (abs(angle) < 45)
{
Motor1.SetSpeed(-leftSpeed);
Motor2.SetSpeed(rightSpeed);
// Serial.print(a);
// Serial.print(",");
// Serial.print(-leftSpeed);
// Serial.print(",");
// Serial.print(-rightSpeed);
// Serial.println(",");
} else {
Motor1.SetSpeed(-0);
Motor2.SetSpeed(-0);
}
}
void ProcessCommand()
{
static unsigned long lastCmdIn = 0;
int c;
if (Serial.available() > 0 )
{
c = Serial.read();
lastCmdIn = millis();
switch (c)
{
case 'U': case 'u':
ctrlwalk = 2;
break;
case 'd':
ctrlwalk = - 2;
break;
case 'L': case 'l':
ctrlTurn = -45;
break;
case 'R': case 'r':
ctrlTurn = 45;
break;
case 'A': case 'a':
break;
case 'C': case 'c':
break;
case 'B': case 'b':
break;
case 'D':
break;
default:
break;
}
} else if (millis() - lastCmdIn > 300) {
}
}
|
Klant 2017-07-04 08:44:17
처음부터 끝까지의 코드 해석을 말씀하시는건가요?
코드를 해석하시기위해서는 밸런싱 로봇에 사용되는 부품에 대한 사용 방법에 대한 이해부터 숙지되어야 한다고 생각합니다.
IMU 센서를 사용하시더라도 라이브러리 함수에 대한 이해가 없이는 위 코드에 대한 이해는 어렵습니다.
사용하시게 되는 센서의 목록을 미리 정리해보시는게 좋을 것 같습니다.
자이로 센서에 대한 사용 방법은 아래 링크를 참고하세요~
https://kocoafab.cc/tutorial/view/239