내가 프로젝트에서 사용하는 델타 로봇은 산업에서 많이쓰이는 병렬 로봇이며 3축 DOF (Degree of Freedom) 를 갖는다.
참고.
직렬로봇 : 1개의 직렬체인(다리)으로 구성되고, 모든 조인트에 구동기가 장착된다.
병렬로봇 : 엔드이펙터가 적어도 2개 이상의 다리에 의하여 지지되고, 각 다리당 1~2개의 구동기가 고정부 근처에 장착되며 나머지 조인트는 수동조인트로 구성된다.
일단 FreeRTOS를 사용했는데 현재는 하나의 Task만 쓰고 있다.
Motor Init() 시점에서 속도를 미리 설정해서 지난번보다 부드럽게 움직이게끔 했고, UART Receive 인터럽트에서 세마포어 시그널을 보내서 시리얼 터미널로 PC에서 제어가 가능하게 끔 해놨다.
구현해놓은 커맨드는 우선
End Effector의 x, y 좌표는 중심점 그대로 두고 UP, DOWN 하는 기능
세 모터의 현재 각도(Position)을 읽어서 UART로 출력해주는 기능
Torque on/off 하는 기능을 추가해 두었다.
나중에는 영상처리를 통한 객체 검출 기능과 MFC를 사용해 티칭시스템을 구현해볼 생각도 있어서 차례차례 이것 저것 붙여나갈 생각이다.
+ 만능기판에 납땜한 버전
납땜초보는 꼭 보안경과 마스크와 장갑을 착용한채로 납땜하길 바란다. 나는 납땜하다가 새끼손가락에 약한 화상을 입었다.
전원은 12V/2A 어댑터를 사용하는걸로 바꿨고 납땜이 깔끔하지 못해서 쇼트 위험이 있을까봐 나중에 추가할거 다 추가하고 글루건으로 마무리 해줄 생각이다.
코드는 AX-12A 모터 아두이노 라이브러리를 참고해 작성했다.
dxl_control.h
/*
* dxl_control.h
*
* Created on: Aug 11, 2022
* Author: J
* thanks to : https://github.com/jumejume1/AX-12A-servo-library
*/
#include "main.h"
#ifndef INC_DXL_CONTROL_H_
#define INC_DXL_CONTROL_H_
#define AX_HEADER 0xFF
#define AX_BROADCAST_ID 0xFE
#define AX_WRITE 0x03
#define AX_READ 0x02
#define AX_READ_ONE_BYTE 1
#define AX_READ_TWO_BYTE 2
#define OFF 0
#define ON 1
#define RX_MODE 0
#define TX_MODE 1
// EEPROM AREA
#define ADDR_MODEL_NUMBER_L 0
#define ADDR_MODEL_NUMBER_H 1
#define ADDR_VERSION 2
#define ADDR_ID 3
#define ADDR_BAUD_RATE 4
#define ADDR_RETURN_DELAY_TIME 5
#define ADDR_CW_ANGLE_LIMIT_L 6
#define ADDR_CW_ANGLE_LIMIT_H 7
#define ADDR_CCW_ANGLE_LIMIT_L 8
#define ADDR_CCW_ANGLE_LIMIT_H 9
#define ADDR_SYSTEM_DATA2 10
#define ADDR_LIMIT_TEMPERATURE 11
#define ADDR_DOWN_LIMIT_VOLTAGE 12
#define ADDR_UP_LIMIT_VOLTAGE 13
#define ADDR_MAX_TORQUE_L 14
#define ADDR_MAX_TORQUE_H 15
#define ADDR_RETURN_LEVEL 16
#define ADDR_ALARM_LED 17
#define ADDR_ALARM_SHUTDOWN 18
#define ADDR_OPERATING_MODE 19
#define ADDR_DOWN_CALIBRATION_L 20
#define ADDR_DOWN_CALIBRATION_H 21
#define ADDR_UP_CALIBRATION_L 22
#define ADDR_UP_CALIBRATION_H 23
// RAM AREA
#define ADDR_TORQUE_ENABLE 24
#define ADDR_LED 25
#define ADDR_CW_COMPLIANCE_MARGIN 26
#define ADDR_CCW_COMPLIANCE_MARGIN 27
#define ADDR_CW_COMPLIANCE_SLOPE 28
#define ADDR_CCW_COMPLIANCE_SLOPE 29
#define ADDR_GOAL_POSITION_L 30
#define ADDR_GOAL_POSITION_H 31
#define ADDR_GOAL_SPEED_L 32
#define ADDR_GOAL_SPEED_H 33
#define ADDR_TORQUE_LIMIT_L 34
#define ADDR_TORQUE_LIMIT_H 35
#define ADDR_PRESENT_POSITION_L 36
#define ADDR_PRESENT_POSITION_H 37
#define ADDR_PRESENT_SPEED_L 38
#define ADDR_PRESENT_SPEED_H 39
#define ADDR_PRESENT_LOAD_L 40
#define ADDR_PRESENT_LOAD_H 41
#define ADDR_PRESENT_VOLTAGE 42
#define ADDR_PRESENT_TEMPERATURE 43
#define ADDR_REGISTERED_INSTRUCTION 44
#define ADDR_PAUSE_TIME 45
#define ADDR_MOVING 46
#define ADDR_LOCK 47
#define ADDR_PUNCH_L 48
#define ADDR_PUNCH_H 49
typedef enum{
FALSE = 0,
TRUE = 1
}bool;
void deltaInit();
void sendInstPacket(uint8_t* packet, uint8_t length);
void servoDelay(uint32_t millisec);
void setMovingSpeed(uint8_t ID, uint16_t Speed);
void setGoalPosition(uint8_t ID, uint16_t Position);
void onOffTorque(uint8_t ID, uint8_t State);
void getPresentPosition(uint8_t ID);
void upEndEffector();
void downEndEffector();
#endif /* INC_DXL_CONTROL_H_ */
dxl_control.c
/*
* dxl_control.c
*
* Created on: Aug 11, 2022
* Author: J
*/
#include "dxl_control.h"
#include "cmsis_os.h"
#include <stdio.h>
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
#define RxBuf_SIZE 20
extern uint8_t RxBuf[RxBuf_SIZE];
char buffer[10]={0,};
void deltaInit(){
setMovingSpeed(AX_BROADCAST_ID, 100);
upEndEffector();
uint8_t str[] = "****** CONTROL MENU ******\r\n1. UP\r\n2. DOWN\r\n3. Read Position\r\n4. Torque Off\r\n5. Torque On\r\n**************************\r\n";
HAL_UART_Transmit(&huart3, str, sizeof(str), 1000);
}
void servoDelay(uint32_t millisec){
osDelay(millisec);
}
void sendInstPacket(uint8_t* packet, uint8_t length)
{
HAL_GPIO_WritePin(Direction_GPIO_Port, Direction_Pin, TX_MODE); // Switch to Transmission Mode
HAL_UART_Transmit(&huart2, packet, length, 1000);
//servoDelay(25);
HAL_GPIO_WritePin(Direction_GPIO_Port, Direction_Pin, RX_MODE); // Switch back to Reception Mode
}
void setMovingSpeed(uint8_t ID, uint16_t Speed)
{
uint8_t Speed_L = Speed;
uint8_t Speed_H = Speed >> 8;
// 16 bits -> 2 x 8 bits
uint8_t length = 9;
uint8_t packet[length];
packet[0] = AX_HEADER;
packet[1] = AX_HEADER;
packet[2] = ID;
packet[3] = length-4;
packet[4] = AX_WRITE;
packet[5] = ADDR_GOAL_SPEED_L;
packet[6] = Speed_L;
packet[7] = Speed_H;
uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
packet[8] = Checksum;
sendInstPacket(packet, length);
}
void setGoalPosition(uint8_t ID, uint16_t Position)
{
uint8_t Position_L = Position;
uint8_t Position_H = Position >> 8;
// 16 bits -> 2 x 8 bits
uint8_t length = 9;
uint8_t packet[length];
packet[0] = AX_HEADER;
packet[1] = AX_HEADER;
packet[2] = ID;
packet[3] = length-4;
packet[4] = AX_WRITE;
packet[5] = ADDR_GOAL_POSITION_L;
packet[6] = Position_L;
packet[7] = Position_H;
uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
packet[8] = Checksum;
sendInstPacket(packet, length);
}
void getPresentPosition(uint8_t ID)
{
uint8_t length = 8;
uint8_t packet[length];
packet[0] = AX_HEADER;
packet[1] = AX_HEADER;
packet[2] = ID;
packet[3] = length-4;
packet[4] = AX_READ;
packet[5] = ADDR_PRESENT_POSITION_L;
packet[6] = AX_READ_TWO_BYTE;
uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6])) & 0xFF;
packet[7] = Checksum;
sendInstPacket(packet, length);
uint8_t buf[30]={0,};
servoDelay(25);
uint16_t presentPosition = RxBuf[5] + (RxBuf[6]<<8);
sprintf((char*)buf, "ID %d's Position : %d\r\n", ID, presentPosition);
HAL_UART_Transmit(&huart3, buf, sizeof(buf), 1000);
}
void onOffTorque(uint8_t ID, uint8_t State){
uint8_t length = 8;
uint8_t packet[length];
packet[0] = AX_HEADER;
packet[1] = AX_HEADER;
packet[2] = ID;
packet[3] = length-4;
packet[4] = AX_WRITE;
packet[5] = ADDR_TORQUE_ENABLE;
packet[6] = State;
uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6])) & 0xFF;
packet[7] = Checksum;
sendInstPacket(packet, length);
}
void upEndEffector(){
setGoalPosition(AX_BROADCAST_ID, 512);
servoDelay(1000);
}
void downEndEffector(){
setGoalPosition(AX_BROADCAST_ID, 630);
servoDelay(1000);
}
'DSP, MCU > STM32 (ARM Cortex-M)' 카테고리의 다른 글
| STM32 ] 역기구학(Inverse kinematics)을 통한 델타로봇 제어 (3) (0) | 2022.08.22 |
|---|---|
| STM32 ] UART DMA, IDLE Line Interrupt 를 통한 데이터 수신 (Ring Buffer) (0) | 2022.08.18 |
| STM32 ] Dynamixel AX-12A 모터 제어하기 (1) (7) | 2022.08.11 |
| STM32 ] FreeRTOS + Semaphore ISR 사용 예제 (0) | 2022.08.08 |
| STM32 ] FreeRTOS 사용해보기 (1) | 2022.08.08 |
