도립 진자 형태의 모바일 로봇을 제어하는 프로젝트입니다
- 로봇이 기울면 모터를 움직여서 중심을 잡는다
- 이것을 위해서 로봇이 기울어진 각도를 알 수 있는 센서 필요
- IMU(관성측정장비) : 가속도계, 회전 속도계, 자력계 등
- MPU6050
- 회전 속도계, 가속도계가 결합된 형태의 센서 모듈
- I2C 통신 인터페이스 포함
- 비례-적분-미분 제어기
- 비례항 : 현재 상태에서의 오차값 크기에 비례한 제어작용
- 적분항 : 오차값 크기를 적분하여 제어작용, 정상상태 오차를 줄이는 역할
- 미분항 : 출력값의 급격한 변화를 줄여 오버슈트를 줄이고 안정성 향상
로봇을 제어하는 하드웨어와 소프트웨어입니다
- MCU, WiFi Module, IMU Sensor를 비롯한 커넥터들이 연결됩니다
Left Header와 Right Header에 CN7, CN10(NUCLEO-F411RE Morpho headers)이 각각 연결됩니다
- DRV8825(Stepper Driver)를 비롯한 커넥터들이 연결됩니다
- Used NUCLEO-F411RE Board
- Used DRV8825
- Used WizFi210
// in the app.c
void PostureControl(void)
{
// 자세 제어를 위한 PID 제어기입니다
}// in the app.c
void CentroidControl(void)
{
// 무게중심 제어를 위한 PID 제어기입니다
}위 함수들은 timer period elapsed callback에서 호출됩니다
// in the interrupt.c
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{
/*... codes ...*/
else if(htim->Instance == TIM2){ // TIM2 frequency = 1KHz
/*... codes ...*/
PostureControl();
/*... codes ...*/
CentroidControl();
/*... codes ...*/
}
/*... codes ...*/
}// in the interrupt.c
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{
if(htim->Instance == TIM3){ // TIM3 frequency = 50KHz
// motor drive task ...
}
/*... codes ...*/
}Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle)
α = τ/(τ + Δt)
(Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt
Δt = sampling rate, τ = time constant greater than timescale of typical accelerometer noise
// in the app.c
void GET_ANGLE(void)
{
/*... codes ...*/
else if(!(flag.set_angle_offset)){
gotAngle = A*(gotAngle + gyroXrate*timePass) + (1 - A)*accXangle; /* Complementary Filter */
}
/*... codes ...*/
}로봇의 상태를 모니터링하면서 제어기를 튜닝할 수 있는 윈도우 앱입니다
- 로봇의 WiFi Module(WizFi210)과
Command Center간의 socket 통신으로 원격 튜닝을 구현합니다 - 두 제어기(Block Diagram 참고)의 gain을 튜닝하기 위해서 Dialog로부터 입력되는 각 gain 값을 struct에 담아 BroadCast() 합니다
typedef struct Gain
{
// variables ...
}Gain;void CCommand_CenterDlg::OnBnClickedButtonSend()
{
// pc2mcu 구조체 변수의 각 멤버 세팅
pListenSocket->BroadCast(&pc2mcu, sizeof(pc2mcu));
// codes ...
}void CListenSocket::BroadCast(PC2MCU* pszBuffer, int len)
{
if(pDlg->isCanSend){
POSITION pos = ChildSocket_List.GetHeadPosition();
CChildSocket* pChild = NULL;
while(pos != NULL){
pChild = (CChildSocket*)ChildSocket_List.GetNext(pos);
if(pChild != NULL)
pChild->Send(pszBuffer, len);
}
pDlg->isCanSend = false;
}
}- WiFi Module은
Command Center로부터 전송된 데이터에서 gain을 읽어 gain을 저장하는 변수를 setting 합니다
void Tuning(void)
{
// Gain 값 세팅
}- 키보드의 방향키를 누르면 그에 따른 이벤트를 처리합니다
BOOL CCommand_CenterDlg::PreTranslateMessage(MSG* pMsg)
{
if(pMsg->message == WM_KEYDOWN){
if(!CheckDriveClicked()){
SetDriveClicked(WM_KEYDOWN);
switch (pMsg->wParam)
{
case VK_LEFT:
m_BtnL.SetCheck(true);
DriveCommand(LEFT);
break;
// codes ...
}
}
}
else if(pMsg->message == WM_KEYUP){
// codes ...
}
return CDialogEx::PreTranslateMessage(pMsg);
}- 어떤 키가 눌렸는지 여부를 체크섬과 함께 로봇에 전송합니다
void CCommand_CenterDlg::DriveCommand(char dir)
{
// codes ...
CreateCheckSum_PC2MCU();
pListenSocket->BroadCast(&pc2mcu, sizeof(pc2mcu));
}void CCommand_CenterDlg::CreateCheckSum_PC2MCU(void)
{
char* ptr = (char*)(&pc2mcu);
int temp = (int)NULL;
for(int i=1; i<sizeof(pc2mcu); i++){
temp += ptr[i];
temp &= 0x000000FF;
}
pc2mcu.checksum = (char)temp;
}https://kocoafab.cc/make/view/719
https://ko.wikipedia.org/wiki/관성_측정_장비
https://www.makerlab-electronics.com/product/triple-axis-accelerometer-gyro-breakout-mpu6050/
https://ko.wikipedia.org/wiki/PID_제어기