ARM-Ultrasonic, Motor

Ultrasonic Driver

TRIG에서 10us의 pulse를 주면 40kHz의 초음파가 발사되고, ECHO핀은 High로 올라간다.
물체에 반사되어 다시 초음파센서로 수신되면 ECHO핀은 Low로 떨어지게 된다.
이때 ECHO핀의 High level시간으로 거리를 측정할 수 있다.

Distance = High level time(ECHO pin) X sound velocity(340m/s) / 2

TRIG핀을 output으로, ECHO핀을 외부인터럽트로 설정한다. TRIG핀에서는 위의 Timing Diagram과 같이 Pulse를 만들고, ECHO핀에서는 인터럽트를 이용해 Rising/Falling Edge에서 인터럽트가 걸리도록 한다. Timer를 이용해 Rising Edge에서 시간측정을 시작하고, Falling Edge에서 시간측정을 멈추고, 시간 데이터를 가져온다.

모든 인터럽트는 NVIC(Nested Vectored Interrupt Controller)를 통해 제어된다.

• Nested는 인터럽트가 발생한 상황에서 더 우선순위가 높은 인터럽트가 발생할 수 있다는 것

• Vectored는 벡터테이블에 쓰여있는대로 인터럽트 발생 시 그 주소로 이동하는 것

• 인터럽트 과정 : 인터럽트가 발생했을 때 해당 인터럽트 벡터테이블에 있는곳으로 가서 안에 입력되있는 해당인터럽트함수(인터럽트 서비스 루틴)의 주소로 간다
  함수코드를 수행하고 다시 원래대로 복귀

핀 숫자가 같은거는 서로 묶여있다.

Timer는 주파수 100MHz를 100으로 prescaling하여 1MHz, 즉 1 CNT 당 1us로 계산하였다.

#include "Ultrasonic.h"
#include "stm32f4xx_hal.h"
#include "../common/delayus/delayus.h"

// trig port, trig pin, echo port, echo pin, timer, flag
typedef struct { // 다르게 쓰인 정보를 구조체로 표현
	TIM_HandleTypeDef *hTim;
	GPIO_TypeDef* GPIO_Trig;
	uint16_t GPIO_TrigPin;
	GPIO_TypeDef* GPIO_Echo;
	uint16_t GPIO_EchoPin;
	uint16_t timCounter;
	int echoFlag;
} ultraSonic_t;

void UltraSonic_init(
		ultraSonic_t *ultraSonic, TIM_HandleTypeDef *hTim,
		GPIO_TypeDef* GPIO_Trig, uint16_t GPIO_TrigPin,
		GPIO_TypeDef* GPIO_Echo, uint16_t GPIO_EchoPin)
{
	ultraSonic->hTim = hTim;
	ultraSonic->GPIO_Trig = GPIO_Trig;
	ultraSonic->GPIO_TrigPin = GPIO_TrigPin;
	ultraSonic->GPIO_Echo = GPIO_Echo;
	ultraSonic->GPIO_EchoPin = GPIO_EchoPin;
	ultraSonic->timCounter = 0;
	ultraSonic->echoFlag = 0;
}

int UltraSonic_getEchopinState(ultraSonic_t *ultraSonic)
{
	return HAL_GPIO_ReadPin(ultraSonic->GPIO_Echo, ultraSonic->GPIO_EchoPin);
}

void UltraSonic_clearTimer(ultraSonic_t *ultraSonic)
{
	__HAL_TIM_SET_COUNTER(ultraSonic->hTim, 0); // counter 0으로 초기화
}

void UltraSonic_startTimer(ultraSonic_t *ultraSonic)
{
	HAL_TIM_Base_Start(ultraSonic->hTim); // count 시작
}

void UltraSonic_stopTimer(ultraSonic_t *ultraSonic)
{
	HAL_TIM_Base_Stop(ultraSonic->hTim); // count 끝
}

uint16_t UltraSonic_getTimerCounter(ultraSonic_t *ultraSonic)
{
	return __HAL_TIM_GET_COUNTER(ultraSonic->hTim); // count값을 가져와 저장
}

void UltraSonic_ISR_Process(ultraSonic_t *ultraSonic, uint16_t GPIO_Pin) //interrupt가 rising/falling edge에서 걸림
{
	if ((GPIO_Pin == ultraSonic->GPIO_EchoPin)) {
		// echo pin high 유지시간 측정
		if (UltraSonic_getEchopinState(ultraSonic)) { // rising edge
			UltraSonic_clearTimer(ultraSonic);
			UltraSonic_startTimer(ultraSonic);
			UltraSonic_clearEchoFlag(ultraSonic);
		}
		else { // falling edge
			UltraSonic_stopTimer(ultraSonic);
			ultraSonic->timCounter = UltraSonic_getTimerCounter(ultraSonic);
			UltraSonic_setEchoFlag(ultraSonic);
		}
	}
}

void UltraSonic_startTrig(ultraSonic_t *ultraSonic)
{
	HAL_GPIO_WritePin(ultraSonic->GPIO_Trig, ultraSonic->GPIO_TrigPin, SET);
	DelayUS(15);
	HAL_GPIO_WritePin(ultraSonic->GPIO_Trig, ultraSonic->GPIO_TrigPin, RESET);
}

void UltraSonic_clearEchoFlag(ultraSonic_t *ultraSonic)
{
	ultraSonic->echoFlag = 0;
}

void UltraSonic_setEchoFlag(ultraSonic_t *ultraSonic)
{
	ultraSonic->echoFlag = 1;
}

int UltraSonic_isCmpltRecvEcho(ultraSonic_t *ultraSonic)
{
	return ultraSonic->echoFlag;
}

int UltraSonic_getDistance(ultraSonic_t *ultraSonic)
{
	int timeout = 0;

	UltraSonic_startTrig(ultraSonic);
	while(!UltraSonic_isCmpltRecvEcho(ultraSonic)){ // 완료되면 탈출
		timeout++;
		if(timeout > 20) return 0;
		HAL_Delay(1);
	}
	UltraSonic_clearEchoFlag(ultraSonic);
	return ultraSonic->timCounter * 0.017; // cm distance
}

UltraSonic_getDistance함수를 사용해 거리를 측정할 수 있다.

Motor Driver

PWM(Pulse Width Modulation)은 주기 내에서 High와 Low의 duty비가 달라짐에 따라 평균 전압이 달라진다.

• PWM Mode 1 : CNT < CCR 에서 High, CNT > CCR 에서 Low를 출력한다.

• Pulse(CCR) : 기준값

• Output compare preload : Enable상태에서 CCR값을 변경했을 때 다음 주기부터 변경값이 적용된다.

enable핀에 pwm을 연결하고, input핀을 조절해 정방향과 역방향을 만들 수 있다.
2개의 Channel(왼쪽 모터, 오른쪽 모터)을 사용

#include "motor.h"
#include "stm32f4xx_hal.h"

typedef struct {
	TIM_HandleTypeDef *htim;
	uint32_t Channel;
	GPIO_TypeDef *dir1_GPIO;
	uint16_t dir1_GPIO_PIN;
	GPIO_TypeDef *dir2_GPIO;
	uint16_t dir2_GPIO_PIN;
}motor_t;

void Motor_init(motor_t *motor,
		TIM_HandleTypeDef *htim,
		uint32_t Channel,
		GPIO_TypeDef *dir1_GPIO,
		uint16_t dir1_GPIO_PIN,
		GPIO_TypeDef *dir2_GPIO,
		uint16_t dir2_GPIO_PIN)
{
	motor->htim = htim;
	motor->Channel = Channel;
	motor->dir1_GPIO = dir1_GPIO;
	motor->dir1_GPIO_PIN = dir1_GPIO_PIN;
	motor->dir2_GPIO = dir2_GPIO;
	motor->dir2_GPIO_PIN = dir2_GPIO_PIN;
}

void Motor_stop(motor_t *motor)
{
	HAL_TIM_PWM_Stop(motor->htim, motor->Channel);
}

void Motor_forward(motor_t *motor)
{
	HAL_GPIO_WritePin(motor->dir1_GPIO, motor->dir1_GPIO_PIN, SET);
	HAL_GPIO_WritePin(motor->dir2_GPIO, motor->dir2_GPIO_PIN, RESET);
	HAL_TIM_PWM_Start(motor->htim, motor->Channel);
}

void Motor_backward(motor_t *motor)
{
	HAL_GPIO_WritePin(motor->dir1_GPIO, motor->dir1_GPIO_PIN, RESET);
	HAL_GPIO_WritePin(motor->dir2_GPIO, motor->dir2_GPIO_PIN, SET);
	HAL_TIM_PWM_Start(motor->htim, motor->Channel);
}

void Motor_setSpeed(motor_t *motor, int speedVal)
{
	__HAL_TIM_SET_COMPARE(motor->htim, motor->Channel, speedVal);
}