296 lines
7.9 KiB
C
296 lines
7.9 KiB
C
#include "Clock.h"
|
|
#include "msp.h"
|
|
#include <stdint.h>
|
|
|
|
//=============================================================================
|
|
// 1. 상수 정의 (Constants)
|
|
//=============================================================================
|
|
// (!!주의!!) SMCLK = 12MHz 기준입니다.
|
|
#define RPM_MAGIC_NUMBER 2000000
|
|
#define TIMER_A0_PWM_PERIOD_TICKS 15000
|
|
#define STALL_TIMER_PERIOD 50000
|
|
|
|
#define LEFT_PWM_PIN BIT7
|
|
#define LEFT_DIR_PIN BIT5
|
|
#define LEFT_SLEEP_PIN BIT7
|
|
|
|
#define RIGHT_PWM_PIN BIT6
|
|
#define RIGHT_DIR_PIN BIT4
|
|
#define RIGHT_SLEEP_PIN BIT6
|
|
|
|
#define TURN_30_DEG_PULSES 60
|
|
#define TURN_SPEED 2000
|
|
|
|
static volatile uint32_t g_left_pulse_period = 0;
|
|
static volatile uint32_t g_right_pulse_period = 0;
|
|
static volatile uint16_t g_last_left_capture = 0;
|
|
static volatile uint16_t g_last_right_capture = 0;
|
|
static volatile uint32_t g_left_pulse_count = 0;
|
|
static volatile uint32_t g_right_pulse_count = 0;
|
|
|
|
static volatile uint8_t g_left_new_data = 0;
|
|
static volatile uint8_t g_right_new_data = 0;
|
|
|
|
/*
|
|
FUNCTION DECLARATIONS
|
|
*/
|
|
void Motor_Init(void);
|
|
void Motor_Left_Forward(uint16_t speed);
|
|
void Motor_Left_Backward(uint16_t speed);
|
|
void Motor_Right_Forward(uint16_t speed);
|
|
void Motor_Right_Backward(uint16_t speed);
|
|
void Motor_Left_Stop(void);
|
|
void Motor_Right_Stop(void);
|
|
void Motor_Stop_All(void);
|
|
|
|
void rpm_tachometer_init(void);
|
|
uint32_t get_left_rpm(void);
|
|
uint32_t get_right_rpm(void);
|
|
void reset_pulse_counts(void);
|
|
uint32_t get_left_pulse_count(void);
|
|
uint32_t get_right_pulse_count(void);
|
|
|
|
static void capture_init_timer_A3(void);
|
|
static void Timer_A2_Stall_Init(void);
|
|
void Rotate_30_Degrees(void);
|
|
|
|
/*
|
|
MAIN FUNCTION
|
|
*/
|
|
int main(void) {
|
|
WDT_A->CTL = WDT_A_CTL_PW | WDT_A_CTL_HOLD;
|
|
|
|
Clock_Init48MHz();
|
|
|
|
Motor_Init();
|
|
rpm_tachometer_init();
|
|
|
|
__enable_irq();
|
|
|
|
__delay_cycles(12000000);
|
|
|
|
Rotate_30_Degrees();
|
|
|
|
while (1) {
|
|
Clock_Delay1ms(1000);
|
|
}
|
|
}
|
|
|
|
/*
|
|
IMPLEMENTATIONS 30DEG ROT. FUNCTION
|
|
*/
|
|
void Rotate_30_Degrees(void) {
|
|
reset_pulse_counts();
|
|
Motor_Left_Forward(TURN_SPEED);
|
|
Motor_Right_Backward(TURN_SPEED);
|
|
|
|
uint8_t left_reached = 0;
|
|
uint8_t right_reached = 0;
|
|
|
|
while (left_reached == 0 || right_reached == 0) {
|
|
if (left_reached == 0) {
|
|
if (get_left_pulse_count() >= TURN_30_DEG_PULSES) {
|
|
Motor_Left_Stop();
|
|
left_reached = 1;
|
|
}
|
|
}
|
|
|
|
if (right_reached == 0) {
|
|
if (get_right_pulse_count() >= TURN_30_DEG_PULSES) {
|
|
Motor_Right_Stop();
|
|
right_reached = 1;
|
|
}
|
|
}
|
|
__delay_cycles(1000);
|
|
}
|
|
|
|
Motor_Stop_All();
|
|
}
|
|
|
|
/*
|
|
IMPLEMENTATION MOTOR RELATIVE FUNCTIONS
|
|
*/
|
|
void Motor_Init(void) {
|
|
P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
|
|
P3->DIR |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
|
|
P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
|
|
P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
|
|
|
|
P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
|
|
P5->DIR |= (LEFT_DIR_PIN | RIGHT_DIR_PIN);
|
|
P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
|
|
P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
|
|
|
|
P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
|
|
P2->DIR |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);
|
|
P2->SEL0 |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);
|
|
P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
|
|
|
|
TIMER_A0->CTL = TIMER_A_CTL_SSEL__SMCLK |
|
|
TIMER_A_CTL_ID__1 |
|
|
TIMER_A_CTL_CLR;
|
|
TIMER_A0->CCR[0] = TIMER_A0_PWM_PERIOD_TICKS;
|
|
|
|
TIMER_A0->CCTL[3] = TIMER_A_CCTLN_OUTMOD_7;
|
|
TIMER_A0->CCR[3] = 0;
|
|
|
|
TIMER_A0->CCTL[4] = TIMER_A_CCTLN_OUTMOD_7;
|
|
TIMER_A0->CCR[4] = 0;
|
|
|
|
TIMER_A0->CTL |= TIMER_A_CTL_MC__UP;
|
|
}
|
|
|
|
void Motor_Left_Forward(uint16_t speed) {
|
|
if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
|
|
P5->OUT &= ~LEFT_DIR_PIN;
|
|
TIMER_A0->CCR[4] = speed;
|
|
}
|
|
|
|
void Motor_Left_Backward(uint16_t speed) {
|
|
if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
|
|
P5->OUT |= LEFT_DIR_PIN;
|
|
TIMER_A0->CCR[4] = speed;
|
|
}
|
|
|
|
void Motor_Right_Forward(uint16_t speed) {
|
|
if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
|
|
P5->OUT &= ~RIGHT_DIR_PIN;
|
|
TIMER_A0->CCR[3] = speed;
|
|
}
|
|
|
|
void Motor_Right_Backward(uint16_t speed) {
|
|
if (speed > TIMER_A0_PWM_PERIOD_TICKS) speed = TIMER_A0_PWM_PERIOD_TICKS;
|
|
P5->OUT |= RIGHT_DIR_PIN;
|
|
TIMER_A0->CCR[3] = speed;
|
|
}
|
|
|
|
void Motor_Left_Stop(void) {
|
|
TIMER_A0->CCR[4] = 0;
|
|
}
|
|
|
|
void Motor_Right_Stop(void) {
|
|
TIMER_A0->CCR[3] = 0;
|
|
}
|
|
|
|
void Motor_Stop_All(void) {
|
|
Motor_Left_Stop();
|
|
Motor_Right_Stop();
|
|
}
|
|
|
|
/*
|
|
IMPLEMENTATION RPM FUNC.
|
|
*/
|
|
void rpm_tachometer_init(void) {
|
|
capture_init_timer_A3();
|
|
Timer_A2_Stall_Init();
|
|
}
|
|
|
|
uint32_t get_left_rpm(void) {
|
|
uint32_t period;
|
|
__disable_irq();
|
|
period = g_left_pulse_period;
|
|
__enable_irq();
|
|
|
|
if (period == 0) return 0;
|
|
return RPM_MAGIC_NUMBER / period;
|
|
}
|
|
|
|
uint32_t get_right_rpm(void) {
|
|
uint32_t period;
|
|
__disable_irq();
|
|
period = g_right_pulse_period;
|
|
__enable_irq();
|
|
|
|
if (period == 0) return 0;
|
|
return RPM_MAGIC_NUMBER / period;
|
|
}
|
|
|
|
void reset_pulse_counts(void) {
|
|
__disable_irq();
|
|
g_left_pulse_count = 0;
|
|
g_right_pulse_count = 0;
|
|
__enable_irq();
|
|
}
|
|
uint32_t get_left_pulse_count(void) {
|
|
uint32_t count;
|
|
__disable_irq();
|
|
count = g_left_pulse_count;
|
|
__enable_irq();
|
|
return count;
|
|
}
|
|
uint32_t get_right_pulse_count(void) {
|
|
uint32_t count;
|
|
__disable_irq();
|
|
count = g_right_pulse_count;
|
|
__enable_irq();
|
|
return count;
|
|
}
|
|
|
|
static void Timer_A2_Stall_Init(void) {
|
|
TIMER_A2->CTL = TIMER_A_CTL_SSEL__SMCLK |
|
|
TIMER_A_CTL_ID__2 |
|
|
TIMER_A_CTL_MC__STOP;
|
|
TIMER_A2->CCTL[0] = TIMER_A_CCTLN_CCIE;
|
|
TIMER_A2->CCR[0] = (STALL_TIMER_PERIOD - 1);
|
|
TIMER_A2->EX0 = TIMER_A_EX0_TAIDEX_5;
|
|
|
|
NVIC->IP[3] = (NVIC->IP[3] & 0xFFFFFF00) | 0x40;
|
|
NVIC->ISER[0] = 1 << TA2_0_IRQn;
|
|
|
|
TIMER_A2->CTL |= TIMER_A_CTL_MC__UP | TIMER_A_CTL_CLR;
|
|
}
|
|
|
|
void TA2_0_IRQHandler(void) {
|
|
TIMER_A2->CCTL[0] &= ~TIMER_A_CCTLN_CCIFG;
|
|
|
|
if (g_left_new_data == 0) g_left_pulse_period = 0;
|
|
else
|
|
g_left_new_data = 0;
|
|
|
|
if (g_right_new_data == 0) g_right_pulse_period = 0;
|
|
else
|
|
g_right_new_data = 0;
|
|
}
|
|
|
|
static void capture_init_timer_A3(void) {
|
|
TIMER_A3->CTL = TIMER_A_CTL_SSEL__SMCLK |
|
|
TIMER_A_CTL_MC__CONTINUOUS |
|
|
TIMER_A_CTL_ID__1 |
|
|
TIMER_A_CTL_CLR;
|
|
P10->SEL0 |= BIT4;
|
|
P10->SEL1 &= ~BIT4;
|
|
P10->DIR &= ~BIT4;
|
|
TIMER_A3->CCTL[0] = TIMER_A_CCTLN_CM_1 | TIMER_A_CCTLN_CCIS_0 |
|
|
TIMER_A_CCTLN_SCS | TIMER_A_CCTLN_CAP | TIMER_A_CCTLN_CCIE;
|
|
P10->SEL0 |= BIT5;
|
|
P10->SEL1 &= ~BIT5;
|
|
P10->DIR &= ~BIT5;
|
|
TIMER_A3->CCTL[1] = TIMER_A_CCTLN_CM_1 | TIMER_A_CCTLN_CCIS_0 |
|
|
TIMER_A_CCTLN_SCS | TIMER_A_CCTLN_CAP | TIMER_A_CCTLN_CCIE;
|
|
NVIC->IP[3] = (NVIC->IP[3] & 0x00FFFFFF) | 0x40000000;
|
|
NVIC->ISER[0] = 1 << TA3_0_IRQn;
|
|
NVIC->IP[4] = (NVIC->IP[4] & 0xFFFFFF00) | 0x40;
|
|
NVIC->ISER[0] = 1 << TA3_N_IRQn;
|
|
}
|
|
|
|
void TA3_0_IRQHandler(void) {
|
|
TIMER_A3->CCTL[0] &= ~TIMER_A_CCTLN_CCIFG;
|
|
uint16_t current_capture = TIMER_A3->CCR[0];
|
|
uint32_t period = (current_capture > g_last_right_capture) ? (current_capture - g_last_right_capture) : ((0xFFFF - g_last_right_capture) + current_capture + 1);
|
|
g_right_pulse_period = period;
|
|
g_right_new_data = 1;
|
|
g_last_right_capture = current_capture;
|
|
g_right_pulse_count++;
|
|
}
|
|
|
|
void TA3_N_IRQHandler(void) {
|
|
if (TIMER_A3->CCTL[1] & TIMER_A_CCTLN_CCIFG) {
|
|
TIMER_A3->CCTL[1] &= ~TIMER_A_CCTLN_CCIFG;
|
|
uint16_t current_capture = TIMER_A3->CCR[1];
|
|
uint32_t period = (current_capture > g_last_left_capture) ? (current_capture - g_last_left_capture) : ((0xFFFF - g_last_left_capture) + current_capture + 1);
|
|
g_left_pulse_period = period;
|
|
g_left_new_data = 1;
|
|
g_last_left_capture = current_capture;
|
|
g_left_pulse_count++;
|
|
}
|
|
} |