먼저 STM32 부분
구조를 좀 바꿨다. Task 는 다음의 3개로 구성된다.
1. 각 조인트에 달린 모터의 각도를 읽고 정기구학을 통해 엔드이펙터의 좌표를 UART로 송신하는 Task (수행 중엔 다른 Task 보다 Priority 가 높을 필요가 있다)
2. 엔드이펙터의 좌표를 변경하라는 command 가 들어왔을 때 역기구학을 통해 모터의 각도를 제어하는 Task
3. UART 수신으로 command 가 들어오면 처리하는 Task
이렇게 3개가 있다.
Queue는 엔드이펙터 좌표값을 넣는 SetQueue와 커맨드 문자열(cahr 배열 20byte)을 담든 cmdQueue가 있다.
Semaphores는 1번 Task에 보내는 신호로 ReadPosSem을 하나 놔두고 다 지웠다.
UART3 인터럽트 콜백함수에서 커맨드 형태 문자열 수신 -> 3번 Task로 cmdQue 전송 -> 3번 Task에서 커맨드 처리-> 좌표변경 커맨드인 경우 2번 Task 로 Queue 전달 / 좌표 읽기 커맨드인 경우 1번 Task 로 Semaphores 신호 생성
uartCallback.c 의 인터럽트 콜백 함수
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart->Instance == USART3){
if(rx_start == 0){
if(rx3_data == 'D'){
bufindex = 0;
rx_start = 1;
}
}
else {
if(rx3_data != '\n' && bufindex < RxBuf_SIZE)
rx3_buf[bufindex++] = rx3_data;
else {
char temp_buf[20];
memcpy(temp_buf, (char*)rx3_buf, 20);
osMessagePut(cmdQueueHandle, (uint32_t)temp_buf, 100);
memset(rx3_buf,0,sizeof(rx3_buf));
bufindex=0;
rx_start = 0;
}
}
HAL_UART_Receive_IT(&huart3, &rx3_data, 1);
}
}
커맨드 규칙 : 'D'로 시작하고 '\n'으로 끝난다.
'D'로 시작하는 경우 rx3_buf에 '\n' 가 들어올 때까지 담고 temp_buf에 옮겨 담아서 Queue 전달을 한 다음에 rx3_buf는 비우고 bufindex와 'D'를 수신했음을 알리는 플래그도 초기화 한다.
freertos.c 의 cmdHandleTask 처리 함수
/* USER CODE BEGIN Header_cmd_Handle_Task */
/**
* @brief Function implementing the cmdHandleTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_cmd_Handle_Task */
void cmd_Handle_Task(void const * argument)
{
/* USER CODE BEGIN cmd_Handle_Task */
/* Infinite loop */
for(;;)
{
osEvent setEvent;
setEvent = osMessageGet(cmdQueueHandle, osWaitForever);
if(setEvent.status == osEventMessage)
{
queueMessage msg;
char cmd[20]={0,};
memcpy(cmd, setEvent.value.p, 20);
if(cmd_handler(cmd, &msg)){
osSemaphoreRelease(ReadPosSemHandle);
osThreadSetPriority(readPosTaskHandle, osPriorityAboveNormal);
}
}
}
/* USER CODE END cmd_Handle_Task */
}
osWaitForever 하고 있다가 메시지를 수신하면 20길이의 char 배열에 memcpy하고 cmd_handler() 함수를 호출하면서 매개변수로 커맨드 문자열과 미리 만들어둔 queueMessage 의 주소를 넘긴다.
queueMessage 를 Task 미리 만들어 주소값을 넘기는 이유는 Queue를 Task 밖에서 만들었을 때 무조건 오류가 났기 때문이다. 심지어 함수화해서 같은 소스파일 내의 아래쪽에 만들어 놨는데도 Task 밖인 경우 무조건 오류가 생기더라.. 그래서 커맨드가 좌표 변경 커맨드일 경우를 대비해서 queueMessage 타입 변수를 넘기고 주소값을 넘긴다 만약 다른 커맨드일 경우엔 이 매개변수가 쓰이지 않는다.
typedef struct
{
float mX;
float mY;
float mZ;
int maxSpeed;
}queueMessage;
그리고 커맨드를 파싱하고 처리한 결과가 좌표 읽기 커맨드인 경우에는 리턴값을 1로 하게끔 해두었다. 이것도 semaphore 시그널 생성을 Task 안에서 해야 문제없이 되기 때문에 이런 구조로 만든 것이다. 그리고 osSemaphoreRelease() 한 다음에 바로 osThreadSetPriority() 함수를 통해 ReadPosTask의 priority를 올리는데, 이런식으로 자신의 priority 뿐만 아니라 다른 Task의 priority 도 올릴 수 있다.
그리고 이건 나도 원인을 찾지 못해서 의아한 부분인데 ISR에서 semaphore release 하고 Task에서 osWaitForever 하고 있으면 절대 안으로 들어오지 않는다. 그래서 osWaitForever 의 두 번째 매개변수를 0으로 해줘야 한다.
근데 ISR이 아닌 다른 Task에서 semaphore release 를 하면 osWaitForever 하고 있다가 시그널을 받고 잘 작동한다. 왜 그런지 너무 궁금한데 아직 못찾아봤다. 알고 있는 사람은 댓글로 알려주면 감사하겠다.
void read_Pos_Task(void const * argument)
{
/* USER CODE BEGIN read_Pos_Task */
/* Infinite loop */
for(;;)
{
if(osSemaphoreWait(ReadPosSemHandle, osWaitForever) == osOK)
{
uint16_t presentPos[3];
for(int i = 0; i < 3; i++){
presentPos[i]=getPresentPosition(i);
memset(rx2_Buf, 0, sizeof(rx2_Buf));
}
char buf[14]="Z+999+999+999\n";
if(presentPos[0]==0 && presentPos[1]==0 && presentPos[2]==0){
}else {
double* tempTheta = ConversionFromServo(presentPos[0], presentPos[1], presentPos[2]);
forward(tempTheta[0],tempTheta[1],tempTheta[2]);
buf[0]='Z';
buf[1]='\0';
for(int i=0; i<3; i++){
if((int)coord[i] >= 0){
strcat(buf, "+");
}else{
strcat(buf, "-");
}
char temp[4]="\0";
sprintf(temp, "%03d",(int)abs(coord[i]));
strcat(buf, temp);
}
strcat(buf, "\n");
}
HAL_UART_Transmit(&huart3, (uint8_t*)buf, sizeof(buf), 1000);
osThreadSetPriority(readPosTaskHandle, osPriorityNormal);
}
}
/* USER CODE END read_Pos_Task */
}
다음으로 ReadPosTask 에서 달라진 부분이 많이 있다.
먼저 getPresentPosition(i) 로 읽은 모터의 각도를 presentPos[i] 에 담고 모터와 연결된 UART 수신버퍼인 rx2_Buf 를 바로 0 으로 memset 한다.
만약 presentPos[i] 가 전부 0인 경우 읽기 오류로 본다.
그 다음 정기구학으로 계산하고 전역변수인 coord[i]의 값을 sprintf함수로 버퍼에 복사해 MFC쪽으로 송신한 뒤 다시 priority를 내린다.
형태는 이런식이다 "Z+035-001-280\n"
cmdHandl.h
/*
* cli.h
*
* Created on: Sep 4, 2022
* Author: Jo soo hyun
*/
#ifndef INC_CLI_H_
#define INC_CLI_H_
#define MAX_CMD_NUM 10
typedef struct
{
float mX;
float mY;
float mZ;
int maxSpeed;
}queueMessage;
typedef int cmd_func(int len, char* cmd, queueMessage* smsg);
struct Command_List
{
char cmd;
cmd_func* func;
};
int cmd_torque(int len, char* cmd, queueMessage* smsg);
int cmd_pump(int len, char* cmd, queueMessage* smsg);
int cmd_conveyorBelt(int len, char* cmd, queueMessage* smsg);
int cmd_pick(int len, char* cmd, queueMessage* smsg);
int cmd_throw(int len, char* cmd, queueMessage* smsg);
int cmd_defaultPos(int len, char* cmd, queueMessage* smsg);
int cmd_read(int len, char* cmd, queueMessage* smsg);
int cmd_moveTo(int len, char* cmd, queueMessage* smsg);
int cmd_handler(char* cmd, queueMessage* smsg);
void deltaInit();
void upEndEffector(queueMessage* smsg);
void downEndEffector(queueMessage* smsg);
void torqueOn();
void torqueOff();
void pumpOn();
void pumpOff();
void cvbeltTurnRight();
void cvbeltTurnLeft();
void cvbeltStop();
#endif /* INC_CLI_H_ */
cmdHandle.c
/*
* cli.c
*
* Created on: Sep 4, 2022
* Author: Jo soo hyun
*
*
*/
#include "cmdHandle.h"
#include "main.h"
#include "dxl_control.h"
#include "DeltaKinematics.h"
#include <stdio.h>
#include <string.h>
#include "stm32f4xx_hal.h"
extern UART_HandleTypeDef huart3;
#include "cmsis_os.h"
extern osMessageQId setQueueHandle;
extern uint16_t GP[3];
int _write(int file, char* p, int len){
HAL_UART_Transmit(&huart3, (uint8_t*)p, len, 10);
return len;
}
struct Command_List CmdList[] =
{
{'T', cmd_torque },
{'P', cmd_pump },
{'C', cmd_conveyorBelt },
{'A', cmd_pick },
{'B', cmd_throw },
{'O', cmd_defaultPos },
{'R', cmd_read },
{'Z', cmd_moveTo },
{0,0 }
};
int cmd_torque(int len, char* cmd, queueMessage* smsg)
{
if(len == 1) {
if (*cmd=='1') {
syncWriteTorqueOnOff(ON);
}
else if (*cmd=='0') {
syncWriteTorqueOnOff(OFF);
}
}else {
printf("wrong command pattern!");
}
return 0;
}
int cmd_pump(int len, char* cmd, queueMessage* smsg)
{
if(len == 1) {
if (*cmd=='1') {
pumpOn();
}
else if (*cmd=='0') {
pumpOff();
}
}else {
printf("wrong command pattern!");
}
return 0;
}
int cmd_conveyorBelt(int len, char* cmd, queueMessage* smsg)
{
if(len == 2) {
if (*cmd=='1') {
if (cmd[1]=='R') {
cvbeltTurnRight();
} else if (cmd[1]=='L'){
cvbeltTurnLeft();
}
}
}else if(len == 1){
if (*cmd=='0') {
cvbeltStop();
}
}else {
printf("wrong command pattern!");
}
return 0;
}
int cmd_pick(int len, char* cmd, queueMessage* smsg)
{
downEndEffector(smsg);
pumpOn();
servoDelay(1000);
upEndEffector(smsg);
return 0;
}
int cmd_throw(int len, char* cmd, queueMessage* smsg)
{
smsg->mX=0;
smsg->mY=-140;
smsg->mZ=-230;
smsg->maxSpeed=1000;
osMessagePut(setQueueHandle, (uint32_t)smsg, 100);
pumpOff();
return 0;
}
int cmd_defaultPos(int len, char* cmd, queueMessage* smsg)
{
upEndEffector(smsg);
return 0;
}
int cmd_moveTo(int len, char* cmd, queueMessage* smsg)
{
if(len == 12){
float tempX = (cmd[1]-'0')*100 + (cmd[2]-'0')*10 + (cmd[3]-'0')*1 ;
if(cmd[0]=='-'){
tempX = -tempX;
}
float tempY = (cmd[5]-'0')*100 + (cmd[6]-'0')*10 + (cmd[7]-'0')*1 ;
if(cmd[4]=='-'){
tempY = -tempY;
}
float tempZ = (cmd[9]-'0')*100 + (cmd[10]-'0')*10 + (cmd[11]-'0')*1 ;
if(cmd[8]=='-'){
tempZ = -tempZ;
}
smsg->mX=tempX;
smsg->mY=tempY;
smsg->mZ=tempZ;
smsg->maxSpeed=100;
osMessagePut(setQueueHandle, (uint32_t)smsg, 100);
}
return 0;
}
int cmd_read(int len, char* cmd, queueMessage* smsg)
{
return 1;
}
int cmd_handler(char* cmd, queueMessage* smsg)
{
struct Command_List* pCmdList = CmdList;
uint8_t command_found = 0;
int read_command_found = 0;
int len = strlen(cmd)-1;
while (pCmdList->cmd)
{
if (pCmdList->cmd==cmd[0])
{
command_found = 1;
read_command_found = pCmdList->func(len, ++cmd, smsg);
break;
}
++pCmdList;
}
if (command_found == 0) printf("command not found!\n");
return read_command_found;
}
void upEndEffector(queueMessage* smsg){
smsg->mX=0;
smsg->mY=0;
smsg->mZ=-256.984;
smsg->maxSpeed=100;
osMessagePut(setQueueHandle, (uint32_t)smsg, 100);
// setGoalPosition(AX_BROADCAST_ID, 510);
// servoDelay(1000);
}
void downEndEffector(queueMessage* smsg){
smsg->mX=0;
smsg->mY=0;
smsg->mZ=-407.891;
smsg->maxSpeed=100;
osMessagePut(setQueueHandle, (uint32_t)smsg, 100);
}
void torqueOn(){
syncWriteTorqueOnOff(ON);
}
void torqueOff(){
syncWriteTorqueOnOff(OFF);
}
void pumpOn(){
HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 0);
}
void pumpOff(){
HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 1);
}
void cvbeltTurnRight(){
setEndless(AX_CONVEYOR_ID, ON);
turn(AX_CONVEYOR_ID, RIGHT, 600);
}
void cvbeltTurnLeft(){
setEndless(AX_CONVEYOR_ID, ON);
turn(AX_CONVEYOR_ID, LEFT, 600);
}
void cvbeltStop(){
onOffTorque(AX_CONVEYOR_ID, OFF);
}
void deltaInit(){
setMovingSpeed(AX_BROADCAST_ID, 100);
cvbeltStop();
//upEndEffector();
setCoordinates(0,0,-256.984);
inverse();
ServoConversion();
setGoalPosition(AX_BROADCAST_ID, GP[0]);
servoDelay(1000);
// uint8_t str[] = "******* CONTROL MENU *******\r\n 1. UP\r\n 2. DOWN\r\n 3. Read Position\r\n 4. Torque Off\r\n 5. Torque On\r\n 6 : Throw(temp)\r\n****************************\r\n";
// HAL_UART_Transmit(&huart3, str, sizeof(str), 1000);
}
MFC 부분에서 추가된 것은 다음과 같다.
슬라이더 컨트롤 초기화 부분
void CdeltaControlDlg::SliderInit(CSliderCtrl* slider)
{
if (slider->GetDlgCtrlID() == IDC_SLIDER_X || slider->GetDlgCtrlID() == IDC_SLIDER_Y) {
slider->SetRange(-100, 100);
slider->SetPos(0);
}
else if (slider->GetDlgCtrlID() == IDC_SLIDER_Z) {
slider->SetRange(-380, -260);
slider->SetPos(-260);
}
slider->SetTicFreq(1);
slider->SetLineSize(10);
}
사실 엔드 이펙터가 이동 가능한 범위는 더 넓지만 슬라이더 컨트롤로는 안전하게 가상의 큐비클 안에서만 움직이도록 슬라이더의 Max, Min 값을 위와 같이 설정했다.
슬라이더 컨트롤의 스크롤이 움직였을 때 메시지 처리 함수
void CdeltaControlDlg::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar)
{
//CSliderCtrl* pSlidCtrl = (CSliderCtrl*)GetDlgItem(IDC_SLIDER_X);
CSliderCtrl* pSlidCtrl = (CSliderCtrl*)pScrollBar;
if (pSlidCtrl->GetDlgCtrlID() == IDC_SLIDER_X) {
m_strX.Format(_T("%d"), pSlidCtrl->GetPos());
}
else if (pSlidCtrl->GetDlgCtrlID() == IDC_SLIDER_Y) {
m_strY.Format(_T("%d"), pSlidCtrl->GetPos());
}
else if (pSlidCtrl->GetDlgCtrlID() == IDC_SLIDER_Z) {
m_strZ.Format(_T("%d"), pSlidCtrl->GetPos());
}
UpdateData(FALSE);
// TODO: 여기에 메시지 처리기 코드를 추가 및/또는 기본값을 호출합니다.
CDialogEx::OnHScroll(nSBCode, nPos, pScrollBar);
}
슬라이더 지정 후 MOVE 버튼을 눌렀을 때의 처리 함수
void CdeltaControlDlg::OnBnClickedBtMove()
{
UpdateData(TRUE);
CString str = _T("DZ");
CString temp;
if (m_sliderX.GetPos() >= 0) {
str += _T("+");
}
else {
str += _T("-");
}
temp.Format(_T("%03d"), abs(m_sliderX.GetPos()));
str += temp;
if (m_sliderY.GetPos() >= 0) {
str += _T("+");
}
else {
str += _T("-");
}
temp.Format(_T("%03d"), abs(m_sliderY.GetPos()));
str += temp;
str += m_strZ;
str += _T("\n");
m_comm->Send(str, str.GetLength());
// TODO: 여기에 컨트롤 알림 처리기 코드를 추가합니다.
}
포트가 안열렸을 때는 시리얼 통신과 관련된 버튼을 비활성화 해두었다.
void CdeltaControlDlg::EnableSerialRelatedControls(bool option)
{
GetDlgItem(IDC_BT_TORQUE)->EnableWindow(option);
GetDlgItem(IDC_BT_PUMP)->EnableWindow(option);
GetDlgItem(IDC_BT_CONVEYOR_ON_R)->EnableWindow(option);
GetDlgItem(IDC_BT_CONVEYOR_ON_L)->EnableWindow(option);
GetDlgItem(IDC_BT_CONVEYOR_OFF)->EnableWindow(option);
GetDlgItem(IDC_BT_DEFAULT_POS)->EnableWindow(option);
GetDlgItem(IDC_BT_PICK)->EnableWindow(option);
GetDlgItem(IDC_BT_THROW)->EnableWindow(option);
GetDlgItem(IDC_BT_MOVE)->EnableWindow(option);
GetDlgItem(IDC_BUTTON_RUN)->EnableWindow(option);
GetDlgItem(IDC_BUTTON_SUSPEND)->EnableWindow(option);
}
그리고 들어온 좌표값을 화면에 표시하는 부분은 아래와 같이 만들었다.
afx_msg LRESULT CdeltaControlDlg::OnReceive(WPARAM length, LPARAM lParam)
{
CString str;
char* data = new char[length + 1];
if (m_comm)
{
m_comm->Receive(data, length); // Length 길이만큼 데이터 받음.
for (int i = 0; i < (int)length; i++)
{
rx.push_back(data[i]);
}
while (rx.size() >= 14)
{
if (rx.at(0) == 'Z' && rx.at(13) == '\n')
{
int tempX = 0;
tempX = (rx.at(2) - '0') * 100 + (rx.at(3) - '0') * 10 + (rx.at(4) - '0') * 1;
if (rx.at(1) == '-') {
tempX = -tempX;
}
int tempY = 0;
tempY = (rx.at(6) - '0') * 100 + (rx.at(7) - '0') * 10 + (rx.at(8) - '0') * 1;
if (rx.at(5) == '-') {
tempY = -tempY;
}
int tempZ = 0;
tempZ = (rx.at(10) - '0') * 100 + (rx.at(11) - '0') * 10 + (rx.at(12) - '0') * 1;
if (rx.at(9) == '-') {
tempZ = -tempZ;
}
if (tempX != 999) {
m_readX.Format(_T("%d"), tempX);
m_readY.Format(_T("%d"), tempY);
m_readZ.Format(_T("%d"), tempZ);
}
else {
m_readX = _T("error");
m_readY = _T("error");
m_readZ = _T("error");
}
UpdateData(false);
rx.erase(rx.begin(), rx.begin() + 13);
}
else if (rx.at(0) != 'Z')
{
rx.erase(rx.begin());
}
else if (rx.at(13) != '\n')
{
rx.erase(rx.begin(), rx.begin() + 13);
}
}
}
delete[] data;
return 0;
}
다음은 티칭기능을 구현하려고 계획중이다.
'DSP, MCU > STM32 (ARM Cortex-M)' 카테고리의 다른 글
| STM32 ] LwIP 사용 초기설정 후 핑테스트 (5) | 2022.09.24 |
|---|---|
| STM32 + MFC ] 델타 로봇 티칭 시스템 구현, 파일입출력 기능 사용 티칭 데이터 관리, 쓰레드 활용 반복작업 수행 (0) | 2022.09.19 |
| STM32 ] 델타로봇, UART로 Command Line Interface 구현 + 기판에 다시 납땜 (5) (0) | 2022.09.04 |
| STM32 ] 델타로봇과 MFC 연동 + 컨베이어 벨트 추가 (4) (0) | 2022.08.28 |
| STM32 ] USB CDC (Virtual Port Com) 사용하기 (0) | 2022.08.26 |
