#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;
}