[ARM] STM32_Radar

1. Radar

 - Ultrasonic 거리 측정, Servo Motor 동작 구현

 - Ultrasonic으로 측정한 거리 UART 출력 및 Python Radar UI 표시(tkinter 사용하여 물체 위치 표시)

(1) : TIM1 set  /  (2) : TIM3 set  /  (3) : GPIO Trigger pin set

(4) : TIM11 set  /  (5) : USART2 set

(1) TIM1 : servo motor 동작 PWM (Pulse frquency : 50Hz(== Period 20ms))

(2) TIM3 : Ultrasonic Input Capture Interrupt TIMER

(3) PA5 : GPIO Trigger, PA6 : TIM3

(4) TIM11 : us delay용 TIMER

(5) UART2 : Interrupt set

 

Data(6byte) 통신 protocol : STX CMD data1 data0 CRC ETX

- protocol 송수신 함수 만들기

#define STX		0x02
#define ETX		0x03

typedef struct {
	uint8_t command;
	uint16_t data;
}protocol_t;

void transmitPacket(protocol_t data);
protocol_t receivePacket();

<UART.c 코드>

// packet 송신
void transmitPacket(protocol_t data)
{
	/*
	 * 사전준비
	 * CRC 계산
	 * 데이터 전송
	 * 데이터 전송 완료 대기
	 */
	// 사전준비
	uint8_t txBuffer[] = {STX, 0, 0, 0, 0, ETX};
	txBuffer[1] = data.command;
	txBuffer[2] = (data.data >> 7) | 0x80;   // big endian
	txBuffer[3] = (data.data & 0x7f) | 0x80;
	// CRC 계산
	txBuffer[4] = txBuffer[0] + txBuffer[1] + txBuffer[2] + txBuffer[3];
	// 데이터 전송
	HAL_UART_Transmit(myHuart, txBuffer, sizeof(txBuffer), 1);
	// 데이터 전송 완료 대기
	while(HAL_UART_GetState(myHuart) == HAL_UART_STATE_BUSY_TX ||
			HAL_UART_GetState(myHuart) == HAL_UART_STATE_BUSY_TX_RX);
	// 데이터 전송 중이면 HAL_UART_STATE_BUSY_TX 상태가 반환 됨
	// 상태가 HAL_UART_STATE_BUSY_TX 끝날 때까지 while문 반복
}

// packet 수신
protocol_t receivePacket()
{
	protocol_t result;
	uint8_t buffer[6];
	uint8_t count = 0;
	uint32_t timeout;

	int16_t ch = getChar();
	memset(&result, 0, sizeof(buffer)); // result 구조체를 0으로 초기화
	if(ch == STX)
	{
		buffer[count++] = ch;
		// STX 수신됐을 때 시간 저장
		timeout = HAL_GetTick();		// System Clock timeTick(32bit)
		while(ch != ETX)
		{
			ch = getChar();
			if(ch != -1)
				buffer[count++] = ch;
			//타임 아웃 처리(노이즈로 STX는 들어왔는데 ETX가 안들어왔을 때) 
			if(HAL_GetTick() - timeout >= 2) return result;
			// 2ms 넘으면 result(0 저장되어 있음) 반환
		}

		// CRC 검사(ETX 정상적으로 수신 됐을 때)
		uint8_t crc = 0;
		for(int i = 0; i<4; i++)
			crc += buffer[i];
		if(crc != buffer[4]) return result;

		// 수신완료 후 데이터 파싱(parsing)
		result.command = buffer[1];
		result.data = buffer[3] & 0x7f;
		result.data |= (buffer[2] & 0x7f) << 7;
	}
	return result;
}

 

코드 리뷰

	txBuffer[2] = (data.data >> 7) | 0x80;    // big endian
	txBuffer[3] = (data.data & 0x7f) | 0x80;

data.data = 16bit

⦁ txBuffer[2] = data를 오른쪽으로 7 shift 하고 1000 0000(2) 을 더해줌 → 1110 1110(2)

⦁ txBuffer[3] = (data & 0111 1111(2)) | 1000 0000(2) = 0010 1001(2) | 1000 0000(2) = 1010 1001(2)

→ data의 상위 2비트는 사용 불가 → 사용 가능한 data 범위 : 0 ~ 0011 1111 1111 1111(2) = 0~16383

(Ultrasonic 측정 가능 범위가 20 ~ 150cm이기 때문에 상관 X)

[코드 동작 flow]
`void transmitPacket(protocol_t data)`
 - Packet 송신 함수
1. 데이터 통신 프로토콜에 따라 버퍼에 데이터 저장 (STX CMD data1 data0 CRC ETX )
     - 16bit data를 8bit(parity bit 1bit + data 7bit)로 쪼개어 저장
     - 해당 버퍼에 저장된 data는 parity bit가 더해졌기에 STX, ETX와 구별 가능
2. 버퍼를 UART 전송
`protocol_t receivePacket() `
 - Packet 수신 및 반환 함수
1. UART를 통해 문자를 하나 수신하고 해당 문자가 STX인지 검사
2. STX가 맞다면 ETX가 수신될 때까지 데이터를 수신하고 버퍼에 저장
3. 데이터 통신 프로토콜에 따라 데이터가 정상적으로 수신되었는지 검사
      - ETX, CRC 검사
      - 데이터가 비정상적이면 함수 종료 및 데이터 반환
4. 데이터 파싱 및  구조체 변수에 저장
      - data에서 parity bit를 제거하고 저장
5. 구조체 변수 반환

---

1-1. 정상 작동 중간 점검(UART 이용)

     - Data 통신 protocol 함수가 정상 동작하는지 확인

     - UART 프로그램으로 protocol에 따라 각도 데이터를 송신하면 Servo Motor가 원하는 각도만큼 동작하는지  확인

     - Ultrasonic으로 측정한 거리를 protocol에 따라 UART 전송하고 프로그램으로 데이터 확인

<main.c 코드>

/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "UART.h"
/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim11;

UART_HandleTypeDef huart2;

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM11_Init(void);
static void MX_USART2_UART_Init(void);

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

long map(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// delay Function implement using TIMER10(for us delay)
void delayUS(uint16_t time)
{
	htim11.Instance->CNT = 0;
	while(htim11.Instance->CNT < time);
}

uint32_t IC_Val1 = 0;
uint32_t IC_Val2 = 0;
uint32_t Difference = 0;
uint8_t IsFirstCaptured = 0;
uint32_t Distance = 0;

// ISR function(Input Capture)
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	// If Rising edge occurs, execute the code
	if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
	{
		// Rising edge Interrupt
		if(IsFirstCaptured == 0)
		{
			// Read Timer CNT
			IC_Val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
			IsFirstCaptured = 1;
			// Next Interrupt -> Falling edge set
			__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1,
					TIM_INPUTCHANNELPOLARITY_FALLING);
		}

		// Falling edge Interrupt
		else if(IsFirstCaptured == 1)
		{
			// Read Timer CNT
			IC_Val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
			htim->Instance->CNT = 0;
			if(IC_Val2 > IC_Val1) Difference = IC_Val2 - IC_Val1;
			// (IC_Val2 - IC_Val1) mean Ultrasonic transmit and receive time difference
			else if(IC_Val1 > IC_Val2) Difference = (0xffff - IC_Val1) + IC_Val2;
			// (IC_Val1 > IC_Val2) mean CNT value overflow

			// calculate distance
			/*
			 * CLK = 1Mhz -> 1clk = 1us
			 * distance = velocity*time = 340[m/s]*(CNT/2)[us] = 0.034*CNT/2[cm]
			 */
			Distance = Difference * 0.034 / 2;
			// Next Interrupt -> Rising edge set
			IsFirstCaptured = 0;
			__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1,
					TIM_INPUTCHANNELPOLARITY_RISING);

			// disable Interrupt
			__HAL_TIM_DISABLE_IT(htim, TIM_IT_CC1);
		}
	}
}

uint32_t getDistance()
{
	// Trigger
	HAL_GPIO_WritePin(Trigger_GPIO_Port, Trigger_Pin, 1);
	delayUS(10);
	HAL_GPIO_WritePin(Trigger_GPIO_Port, Trigger_Pin, 0);

	// enable Interrupt -> can only occur Interrupt when I want to
	__HAL_TIM_ENABLE_IT(&htim3, TIM_IT_CC1);
	return Distance;
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();
  MX_TIM3_Init();
  MX_TIM11_Init();
  MX_USART2_UART_Init();
  /* USER CODE BEGIN 2 */
  initUart(&huart2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_1);
  HAL_TIM_Base_Start(&htim11);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
  	protocol_t txData, rxData;
  	rxData = receivePacket();
  	if(rxData.command == 'R')
  	{
  		htim1.Instance->CCR1 = map(rxData.data, 0, 180, 500, 2300);
  		/*
  		 * long map(long x, long in_min, long in_max, long out_min, long out_max) 함수
  		 * x : mapping 할 data
  		 * in_min : 현재 data 범위의 최소값
  		 * in_max : 현재 data 범위의 최대값
  		 * out_min : mapping 할 범위의 최소값
  		 * out_max : mapping 할 범위의 최대값
  		 * => 현재 데이터의 범위를 바꿔주는(mapping) 함수
  		 * 
  		 * 위에서는 0~180(도) 범위의 rxData.data를 500~2300 범위로 변경하여 pulse width 조절
  		 */
  		txData.command = 'A';
  		txData.data = getDistance();
  		transmitPacket(txData);
  	}
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}
...

0도 data packet : 02 52 80 80 54 03

90도 data packet : 02 52 80 DA AE 03

175도 data packet : 02 52 81 AF 84 03

 

175(10) → 1010 1111(2)

data[0] = 1000 0001(2) = 81(16)  → MSB = parity bit

data[1] = 1010 1111(2) = AF(16)

⇒ UART로 data 송신하면 servo motor 일정 각도 움직임 + UART에 Ultrasonic data 수신

 

⦁ 수신 데이터 02 41 80 80 43 03 이라면,

02 03 : STX, ETX

80 80 : Ultrasonic 거리 data

43 : CRC

 

코드 리뷰

[ARM] STM32_Buzzer, Ulatrasonic, 필터링, ServoMotor

`void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)` 함수에 대한 설명은 위 블로그 확인

  while (1)
  {
  	protocol_t txData, rxData;
  	rxData = receivePacket();
  	if(rxData.command == 'R')
  	{
  		htim1.Instance->CCR1 = map(rxData.data, 0, 180, 500, 2300);
  		/*
  		 * long map(long x, long in_min, long in_max, long out_min, long out_max) 함수
  		 * x : mapping 할 data
  		 * in_min : 현재 data 범위의 최소값
  		 * in_max : 현재 data 범위의 최대값
  		 * out_min : mapping 할 범위의 최소값
  		 * out_max : mapping 할 범위의 최대값
  		 * => 현재 데이터의 범위를 바꿔주는(mapping) 함수
  		 * 
  		 * 위에서는 0~180(도) 범위의 rxData.data를 500~2300 범위로 변경하여 pulse width 조절
  		 */
  		txData.command = 'A';
  		txData.data = getDistance();
  		transmitPacket(txData);
  	}

1. UART.c의 `receivePacket()` 함수를 이용하여 UART 프로그램으로 사용자가 보낸 데이터 수신, 저장

    - 데이터에는 Command Data와 Degree Data(0~180)가 저장되어 있음 

2. 0~180 범위의 Degree Data를 500~2300 범위로 변환하고 CCR에 저장하여 Pulse Width 변경

    - Servo Motor 동작 Pulse Width 범위 = 500 ~ 2300

    - Degree Data 만큼 Servo Motor 동작

3. `getDistance()` 함수를 이용하여 Ulatrasonic으로 측정한 거리 저장

4. 3에서 측정한 거리를 `transmitPacket()` 함수로 packet하여 UART 송신

---

1-2. Python_tkinter(UI 표시)

 

반시계 방향으로 움직이는 막대

import tkinter as tk    # UI 라이브러리
import math

WIDTH = 640
HEIGHT = 480
angle = 0

# 개체 호출
root = tk.Tk()
root.title("Ultrasonic Radar")
canvas = tk.Canvas(root, width=WIDTH, height=HEIGHT, bg="black")
canvas.pack()

# 함수 생성
def updateScan():
    global angle    # 변수 값을 변경해야 해서 global, 참조만 할거면 없어도됨
    
    # 화면 지우기
    canvas.delete('all')
    
    angle = angle + 1
    if angle > 359:
        angle = 0
    x = 320 + math.cos(angle * math.pi / 180) * 200
    y = 240 - math.sin(angle * math.pi / 180) * 200
    """
    cos(degree) = x/radius
    x = cos(degree) * radius
    
    sin(degree) = y/radius
    y = sin(degree) * radius
    
    cos(x) : -1 ~ +1
    cos(x) * 200 : -200 ~ +200
    320 + cos(x) * length : +120 ~ +520
    -> 막대기의 x축 범위가 120~520
    """
    canvas.create_line(320, 240, x, y, fill='red', width=6)
    # 320,240 = 중앙값
    # 화면 왼쪽 위가 0,0
    # x,y가 막대기 끝 점

    # 재귀 호출
    canvas.after(50, updateScan)    # 50ms에 한번씩 update

# 화면 표시
updateScan()
root.mainloop()

 

레이더처럼 움직이는 막대

import tkinter as tk    # UI 라이브러리
import math

WIDTH = 640
HEIGHT = 480
angle = 0
direction = 0

# 개체 호출
root = tk.Tk()
root.title("Ultrasonic Radar")
canvas = tk.Canvas(root, width=WIDTH, height=HEIGHT, bg="black")
canvas.pack()

# 함수 생성
def updateScan():
    global angle    # 변수 값을 변경해야 해서 global, 참조만 할거면 없어도됨
    global direction
    
    # 화면 지우기
    canvas.delete('all')
    
    # 레이더 선 그리기
    radius = WIDTH / 2
    x = radius + math.cos(angle * math.pi / 180) * radius
    y = radius - math.sin(angle * math.pi / 180) * radius
    canvas.create_line(x, y, radius, radius, fill='green', width=4)

    # 각도 업데이트
    if direction == 0:
        angle += 1
        if angle == 181:
            direction = 1
    else:
        angle -= 1
        if angle == -1:
            direction = 0

    # 재귀 호출
    canvas.after(10, updateScan)    # 10ms에 한번씩 update

# 화면 표시
updateScan()
root.mainloop()

 

점 찍기 

import tkinter as tk    # UI 라이브러리
import math

WIDTH = 640
HEIGHT = 480
angle = 0
direction = 0
objects = [[0,0],[10,0],[20,0],[30,0],[40,0],[50,0],[60,0],[70,110],[80,0],
           [90,0],[100,0],[110,0],[120,0],[130,0],[140,00],[150,0],[160,0],
           [170,150],[180,0]]     # 2차원 리스트

# 개체 호출
root = tk.Tk()
root.title("Ultrasonic Radar")
canvas = tk.Canvas(root, width=WIDTH, height=HEIGHT, bg="black")
canvas.pack()

def drawObject(angle, distance):
    radius = WIDTH / 2
    x = radius + math.cos(angle * math.pi / 180) * distance
    y = radius - math.sin(angle * math.pi / 180) * distance
    canvas.create_oval(x-5, y-5, x+5, y+5, fill='green') # 사각형 안의 원 그려줌
    

# 함수 생성
def updateScan():
    global angle    # 변수 값을 변경해야 해서 global, 참조만 할거면 없어도됨
    global direction
    global objects
    
    # 화면 지우기
    canvas.delete('all')
    
    # 레이더 선 그리기
    radius = WIDTH / 2     # 기준점
    length = radius
    x = radius + math.cos(angle * math.pi / 180) * length
    """
    cos(x) : -1 ~ +1
    cos(x) * length : -320 ~ +320
    radius + cos(x) * length : 0 ~ +640
    -> 막대기의 x축 범위가 0~640
    """
    y = radius - math.sin(angle * math.pi / 180) * length
    canvas.create_line(x, y, radius, radius, fill='green', width=4)

    # 물체 그리기
    for obj in objects:
        drawObject(obj[0], obj[1])
    """
    drawObject(0, 1) -> (10, 0) -> (20, 0) ... 리스트 끝날 때까지 반복
    """

    # 각도 업데이트
    if direction == 0:
        angle += 1
        if angle == 181:
            direction = 1
    else:
        angle -= 1
        if angle == -1:
            direction = 0

    # 재귀 호출
    canvas.after(10, updateScan)    # 10ms에 한번씩 update

# 화면 표시
updateScan()
root.mainloop()

 

 

Ultrasonic 센서에서 측정한 거리를 점으로 찍기 + Servo Motor가 막대기와 같이 동작

import tkinter as tk    # UI 라이브러리
import math
import serial
import time

WIDTH = 640
HEIGHT = 480
angle = 0
direction = 0
objects = [[0,0],[10,0],[20,0],[30,0],[40,0],[50,0],[60,0],[70,0],[80,0],
           [90,0],[100,0],[110,0],[120,0],[130,0],[140,0],[150,0],[160,0],
           [170,0],[180,0]]     # 2차원 리스트

ser = serial.Serial("COM11", 115200)

# 개체 호출
root = tk.Tk()
root.title("Ultrasonic Radar")
canvas = tk.Canvas(root, width=WIDTH, height=HEIGHT, bg="black")
canvas.pack()

def drawObject(angle, distance):
    radius = WIDTH / 2
    x = radius + math.cos(angle * math.pi / 180) * distance
    y = radius - math.sin(angle * math.pi / 180) * distance
    canvas.create_oval(x-5, y-5, x+5, y+5, fill='green') # 사각형 안의 원 그려줌
    

# 함수 생성
def updateScan():
    global angle    # 변수 값을 변경해야 해서 global, 참조만 할거면 없어도됨
    global direction
    global objects
    global sendingAngle
    receiveDistance = 0

    #각도 전송(0도 10도 20도... 10도 단위로 Servo Motor 동작)
    if angle % 10 == 0:     # angle==10 일때만(100ms에 한번씩) 실행
                            # (리스트 angle 10 단위)
        sendingAngle = angle
        mask = b'\x7f'
        ser.write(bytes(bytearray([0x02, 0x52])))
        angleH = (angle >> 7) + 128
        angleL = (angle & mask[0]) + 128   # 128==0b1000 0000
        crc = (0x02 + 0x52 + angleH + angleL) % 256     # 1byte 범위
        ser.write(bytes(bytearray([angleH, angleL, crc, 0x03])))

    # 거리 수신
    if ser.in_waiting > 0:
        data = ser.read()
        if data == b'\x02':
            # 두번째 바이트 수신대기
            timeout = time.time() + 0.002   # 2ms
            lostData = False
            while ser.in_waiting < 5:    # 5글자 대기
                # 타임아웃 처리
                if time.time() > timeout:   # 2ms 넘으면 lost data(=error)
                    lostData = True
                    break
            if lostData == False:
                data = ser.read(5)  # CMD ~ ETX (STX는 위에서 검사 했음)
                if data[0] == 65:
                    # CRC 검사
                    crc = (2 + data[0] + data[1] + data[2]) % 256
                    if crc == data[3]:
                        if data[4] == 3:    # ETX 검사
                            # 데이터 파싱
                            mask = b'\x7f'
                            data_one = bytes([data[1] & mask[0]])
                            receiveDistance = int.from_bytes(data_one) << 7
                            data_one = bytes([data[2] & mask[0]])
                            receiveDistance += int.from_bytes(data_one)
                            # 물체 위치(리스트) 업데이트
                            for obj in objects:
                                if obj[0] == sendingAngle:
                                    obj[1] = receiveDistance
            
    # 화면 지우기
    canvas.delete('all')
    
    # 레이더 선 그리기
    radius = WIDTH / 2     # 기준점
    length = radius
    x = radius + math.cos(angle * math.pi / 180) * length
    """
    cos(degree) = x/radius
    x = cos(degree) * radius
    
    sin(degree) = y/radius
    y = sin(degree) * radius
    
    cos(x) : -1 ~ +1
    cos(x) * length : -320 ~ +320
    radius + cos(x) * length : 0 ~ +640
    -> 막대기의 x축 범위가 0~640
    """
    y = radius - math.sin(angle * math.pi / 180) * length
    canvas.create_line(x, y, radius, radius, fill='green', width=4)

    # 물체 그리기
    for obj in objects:
        drawObject(obj[0], obj[1])
    """
    drawObject(0, 1) -> (10, 0) -> (20, 0) ... 리스트 끝날 때까지 반복
    """

    # 각도 업데이트
    if direction == 0:
        angle += 1
        if angle == 181:
            direction = 1
    else:
        angle -= 1
        if angle == -1:
            direction = 0

    # 재귀 호출
    canvas.after(10, updateScan)    # 10ms에 한번씩 update

# 화면 표시
updateScan()
root.mainloop()

(1) : Servo Motor 동작  /  (2) : Servo Motor에 Ultrasonic을 연결하면 Radar처럼 움직임

 

---

 

 

 

 

 

 

 

Made By Minseok KIM