/Designs/HAM Constructions/Radiotelescope/SW/ARM/src/rtdriver.c |
---|
0,0 → 1,281 |
#include <libopencm3/stm32/f1/rcc.h> |
#include <libopencm3/stm32/f1/gpio.h> |
#include <libopencm3/stm32/timer.h> |
#include <libopencm3/stm32/nvic.h> |
#include <libopencm3/stm32/usart.h> |
#include <stdio.h> |
#include <errno.h> |
int pulse_width_abs = 130; |
int pulse_width_speed_div = 15; |
int min_speed = 500; |
int max_speed = 2000; |
int accel = 800; |
/* sinewave in degrees */ |
int8_t sinewave[360] = {0, 2, 4, 6, 8, 11, 13, 15, 17, 19, 22, 24, 26, 28, 30, 32, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 72, 74, 76, 78, 79, 81, 83, 84, 86, 88, 89, 91, 92, 94, 95, 97, 98, 100, 101, 102, 104, 105, 106, 107, 108, 109, 111, 112, 113, 114, 115, 116, 116, 117, 118, 119, 120, 120, 121, 122, 122, 123, 123, 124, 124, 125, 125, 125, 126, 126, 126, 126, 126, 126, 126, 127, 126, 126, 126, 126, 126, 126, 126, 125, 125, 125, 124, 124, 123, 123, 122, 122, 121, 120, 120, 119, 118, 117, 116, 116, 115, 114, 113, 112, 111, 109, 108, 107, 106, 105, 104, 102, 101, 100, 98, 97, 95, 94, 92, 91, 89, 88, 86, 84, 83, 81, 79, 78, 76, 74, 72, 71, 69, 67, 65, 63, 61, 59, 57, 55, 53, 51, 49, 47, 45, 43, 41, 39, 37, 35, 32, 30, 28, 26, 24, 22, 19, 17, 15, 13, 11, 8, 6, 4, 2, 0, -2, -4, -6, -8, -11, -13, -15, -17, -19, -22, -24, -26, -28, -30, -32, -35, -37, -39, -41, -43, -45, -47, -49, -51, -53, -55, -57, -59, -61, -63, -65, -67, -69, -71, -72, -74, -76, -78, -79, -81, -83, -84, -86, -88, -89, -91, -92, -94, -95, -97, -98, -100, -101, -102, -104, -105, -106, -107, -108, -109, -111, -112, -113, -114, -115, -116, -116, -117, -118, -119, -120, -120, -121, -122, -122, -123, -123, -124, -124, -125, -125, -125, -126, -126, -126, -126, -126, -126, -126, -127, -126, -126, -126, -126, -126, -126, -126, -125, -125, -125, -124, -124, -123, -123, -122, -122, -121, -120, -120, -119, -118, -117, -116, -116, -115, -114, -113, -112, -111, -109, -108, -107, -106, -105, -104, -102, -101, -100, -98, -97, -95, -94, -92, -91, -89, -88, -86, -84, -83, -81, -79, -78, -76, -74, -72, -71, -69, -67, -65, -63, -61, -59, -57, -55, -53, -51, -49, -47, -45, -43, -41, -39, -37, -35, -32, -30, -28, -26, -24, -22, -19, -17, -15, -13, -11, -8, -6, -4, -2}; |
const int pwm_freq = 56250; /* 56.25 kHz */ |
void usart_setup(void) |
{ |
rcc_peripheral_enable_clock(&RCC_APB2ENR, |
RCC_APB2ENR_IOPAEN | RCC_APB2ENR_AFIOEN | RCC_APB2ENR_USART1EN); |
gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); |
usart_set_baudrate(USART1, 9600); |
usart_set_databits(USART1, 8); |
usart_set_stopbits(USART1, USART_STOPBITS_1); |
usart_set_mode(USART1, USART_MODE_TX_RX); |
usart_set_parity(USART1, USART_PARITY_NONE); |
usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE); |
usart_enable(USART1); |
} |
int _write(int file, char *ptr, int len) |
{ |
int i; |
if (file == 1) { |
for (i = 0; i < len; i++) { |
usart_send_blocking(USART1, ptr[i]); |
} |
return i; |
} |
errno = EIO; |
return -1; |
} |
int _read(int file, char *ptr, int len) |
{ |
int i; |
if (file == 0) { |
for (i = 0; i < len; i++) { |
*ptr++ = usart_recv_blocking(USART1); |
} |
return i; |
} |
errno = EIO; |
return -1; |
} |
uint32_t *phase_reg[4]; |
int phases_c = sizeof(phase_reg) / sizeof(uint32_t *); |
void set_angle(unsigned int deg, unsigned int pulse_width) |
{ |
unsigned int i; |
for (i = 0; i < phases_c; i++) { |
int16_t val = sinewave[(deg + i * (360 / phases_c)) % 360]; |
val *= pulse_width; |
val /= 255; |
*(phase_reg[i]) = (val > 0 ? val : 0); |
} |
} |
int pulse_width; |
int counter = 0; |
int last_change = 0; |
int stop = 0; |
int speed = 0; |
int pos = 0; |
int tgt_pos = 0; |
int curr_accel = 0; |
int change = 0; |
int calc_speed() |
{ |
int dir = pos < tgt_pos ? 1 : -1; |
unsigned int abs_speed = speed > 0 ? speed : -speed; |
if (pos == tgt_pos && abs_speed <= min_speed) { |
last_change = counter; |
return 0; |
} |
curr_accel = 0; |
/* FIXME: buggy */ |
if (speed * dir < 0) { |
curr_accel = accel * dir; |
} else if (abs_speed * abs_speed >= ((tgt_pos - pos) * dir - 1) * accel * 2) { |
curr_accel = accel * -dir; |
} else { |
if (abs_speed < max_speed) |
curr_accel = accel * dir; |
if (abs_speed > max_speed) |
curr_accel = accel * -dir; |
} |
if (curr_accel) { |
change = curr_accel * (counter - last_change) / pwm_freq; |
last_change += change * pwm_freq / curr_accel; |
return speed + change; |
} else { |
last_change = counter; |
return speed; |
} |
} |
void tim4_isr() |
{ |
if (stop) { |
TIM4_CCR1 = 0; |
TIM4_CCR2 = 0; |
TIM4_CCR3 = 0; |
TIM4_CCR4 = 0; |
TIM_SR(TIM4) &= ~TIM_SR_UIF; |
return; |
} |
counter++; |
int abs_speed = speed > 0 ? speed : -speed; |
if (speed) { |
if (counter % (pwm_freq / abs_speed) == 0) { |
pos += speed > 0 ? 1 : -1; |
} |
} |
if (counter % (pwm_freq / 1000) == 0) { |
speed = calc_speed(); |
} |
pulse_width = pulse_width_abs + abs_speed / pulse_width_speed_div; |
/* TODO */ |
set_angle((pos + 100000) % 360, pulse_width < 255 ? pulse_width : 255); |
TIM_SR(TIM4) &= ~TIM_SR_UIF; |
} |
void set_tgt_pos(int pos) |
{ |
tgt_pos = pos; |
} |
void timer_setup() |
{ |
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN); |
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN); |
gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, |
GPIO6 | GPIO7 | GPIO8 | GPIO9); |
/* 72MHz / 10 = 7.2MHz */ |
TIM_PSC(TIM4) = 9; |
/* 7.2MHz / 128 = 56.250kHz */ |
TIM_CNT(TIM4) = 0; |
TIM_ARR(TIM4) = 128; |
TIM_EGR(TIM4) = TIM_EGR_UG; |
TIM_CCMR1(TIM4) = TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC1PE |
| TIM_CCMR1_OC2M_PWM1 | TIM_CCMR1_OC2PE; |
TIM_CCMR2(TIM4) = TIM_CCMR2_OC3M_PWM1 | TIM_CCMR2_OC3PE |
| TIM_CCMR2_OC4M_PWM1 | TIM_CCMR2_OC4PE; |
TIM_CCER(TIM4) = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E; |
TIM_CCR1(TIM4) = 0; |
TIM_CCR2(TIM4) = 0; |
TIM_CCR3(TIM4) = 0; |
TIM_CCR4(TIM4) = 0; |
TIM_DIER(TIM4) = TIM_DIER_UIE; |
nvic_enable_irq(NVIC_TIM4_IRQ); |
nvic_set_priority(NVIC_TIM4_IRQ, 1); |
TIM_CR1(TIM4) |= TIM_CR1_CEN; |
} |
int main(void) |
{ |
int i; |
setvbuf(stdin, NULL, _IONBF, 0); |
setvbuf(stdout, NULL, _IONBF, 0); |
rcc_clock_setup_in_hse_8mhz_out_72mhz(); |
phase_reg[0] = &TIM4_CCR1; |
phase_reg[1] = &TIM4_CCR2; |
phase_reg[2] = &TIM4_CCR3; |
phase_reg[3] = &TIM4_CCR4; |
speed = 0; |
set_tgt_pos(0); |
usart_setup(); |
timer_setup(); |
char c; |
while (1) { |
c = getchar(); |
switch(c) { |
case 's': |
stop = 1; |
break; |
case 'r': |
printf("current position is %d\r\n", pos); |
printf("enter target position: "); |
scanf("%d", &tgt_pos); |
printf("\r\n"); |
printf("target position is %d\r\n", tgt_pos); |
set_tgt_pos(tgt_pos); |
break; |
case 'v': |
printf("current pulse_width_abs is %d\r\n", pulse_width_abs); |
printf("enter new pulse_width_abs: "); |
scanf("%d", &pulse_width_abs); |
printf("\r\n"); |
break; |
case 'b': |
printf("current pulse_width_speed_div is %d\r\n", pulse_width_speed_div); |
printf("enter new pulse_width_speed_div: "); |
scanf("%d", &pulse_width_speed_div); |
printf("\r\n"); |
break; |
case 'n': |
printf("current min_speed is %d\r\n", min_speed); |
printf("enter new min_speed: "); |
scanf("%d", &min_speed); |
printf("\r\n"); |
break; |
case 'm': |
printf("current max_speed is %d\r\n", max_speed); |
printf("enter new max_speed: "); |
scanf("%d", &max_speed); |
printf("\r\n"); |
break; |
default: |
printf("pos: %d tgt_pos: %d speed: %d pulse_width: %d curr_accel: %d change: %d last_change: %d\r\n", |
pos, tgt_pos, speed, pulse_width, curr_accel, change, last_change); |
} |
} |
return 0; |
} |