281 lines
7.5 KiB
C
281 lines
7.5 KiB
C
#include <libopencm3/stm32/rcc.h>
|
|
#include <libopencm3/stm32/gpio.h>
|
|
#include <libopencm3/stm32/usart.h>
|
|
|
|
#include <libopencm3/cm3/systick.h>
|
|
|
|
enum direction {
|
|
DIR_CLOCKWISE,
|
|
DIR_COUNTERCLOCKWISE
|
|
};
|
|
|
|
volatile struct {
|
|
bool active;
|
|
bool done;
|
|
int step_count;
|
|
enum direction dir;
|
|
int counter;
|
|
int increment;
|
|
int threshold;
|
|
int phase;
|
|
} stepper = {
|
|
true, false, 1000000, DIR_CLOCKWISE, 0, 1, 2, 0
|
|
};
|
|
|
|
volatile struct {
|
|
bool active;
|
|
bool done;
|
|
enum direction dir;
|
|
int count;
|
|
int tocount;
|
|
} brushed = {
|
|
false, false, DIR_CLOCKWISE, 0, 0
|
|
};
|
|
|
|
int main(void) {
|
|
rcc_periph_clock_enable(RCC_GPIOA);
|
|
rcc_periph_clock_enable(RCC_GPIOB);
|
|
|
|
rcc_periph_clock_enable(RCC_USART2);
|
|
|
|
// setup PA5-7 and PB1 for the stepper motor gates
|
|
gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
|
|
gpio_clear(GPIOB, GPIO1);
|
|
gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO5);
|
|
gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6);
|
|
gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO7);
|
|
gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO1);
|
|
|
|
// setup PA2/3 for USART2_RX and USART2_TX
|
|
gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO2|GPIO3);
|
|
gpio_set_af(GPIOA, GPIO_AF4, GPIO2|GPIO3);
|
|
|
|
// setup USART2 stuff
|
|
usart_set_baudrate(USART2, 115200);
|
|
usart_set_databits(USART2, 8);
|
|
usart_set_stopbits(USART2, USART_STOPBITS_1);
|
|
usart_set_mode(USART2, USART_MODE_TX_RX);
|
|
usart_set_parity(USART2, USART_PARITY_NONE);
|
|
usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE);
|
|
usart_enable(USART2);
|
|
|
|
#if 0
|
|
// if I need more speed than the internal 2 MHz
|
|
// setup PA0 as a clock input
|
|
rcc_bypass_enable(RCC_HSE);
|
|
rcc_osc_on(RCC_HSE);
|
|
|
|
rcc_wait_for_osc_ready(RCC_HSE);
|
|
rcc_set_sysclk_source(RCC_HSE);
|
|
#endif
|
|
|
|
// setup systick interrupt
|
|
systick_interrupt_disable();
|
|
systick_counter_disable();
|
|
|
|
systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
|
|
systick_set_reload(rcc_ahb_frequency/1000-1);
|
|
systick_clear();
|
|
|
|
systick_interrupt_enable();
|
|
systick_counter_enable();
|
|
|
|
int32_t val = 0;
|
|
|
|
while (1) {
|
|
if (stepper.done) {
|
|
stepper.done = false;
|
|
usart_send_blocking(USART2, 's');
|
|
}
|
|
if (brushed.done) {
|
|
brushed.done = false;
|
|
usart_send_blocking(USART2, 'r');
|
|
}
|
|
|
|
|
|
if ((USART_ISR(USART2) & USART_ISR_RXNE) != 0) {
|
|
// TODO process byte
|
|
uint8_t rx = usart_recv(USART2);
|
|
|
|
// if it's a hexadecimal echo back and append it to val
|
|
if ((rx >= '0') && (rx <= '9')) {
|
|
usart_send_blocking(USART2, rx); // echo back
|
|
val <<= 4;
|
|
val |= (rx - '0');
|
|
} else if ((rx >= 'a') && (rx <= 'f')) {
|
|
usart_send_blocking(USART2, rx); // echo back
|
|
val <<= 4;
|
|
val |= (rx - 'a' + 10);
|
|
} else if ((rx >= 'A') && (rx <= 'F')) {
|
|
usart_send_blocking(USART2, rx); // echo back
|
|
val <<= 4;
|
|
val |= (rx - 'A' + 10);
|
|
} else {
|
|
switch (rx) {
|
|
case 's': // get status
|
|
if (stepper.active) {
|
|
usart_send_blocking(USART2, '!');
|
|
} else {
|
|
usart_send_blocking(USART2, '@');
|
|
}
|
|
if (brushed.active) {
|
|
usart_send_blocking(USART2, '#');
|
|
} else {
|
|
usart_send_blocking(USART2, '$');
|
|
}
|
|
break;
|
|
case 'w': // start stepper
|
|
usart_send_blocking(USART2, '!');
|
|
stepper.active = true;
|
|
break;
|
|
case 'W': // stop stepper
|
|
stepper.active = false;
|
|
usart_send_blocking(USART2, '@');
|
|
break;
|
|
case 'r': // start brushed motor
|
|
usart_send_blocking(USART2, '#');
|
|
brushed.count = 0;
|
|
brushed.active = true;
|
|
break;
|
|
case 'R': // stop brushed motor
|
|
usart_send_blocking(USART2, '$');
|
|
brushed.count = 0;
|
|
brushed.active = false;
|
|
break;
|
|
case 't': // read stepper counter
|
|
// TODO
|
|
break;
|
|
case 'T': // write stepper counter
|
|
// TODO
|
|
break;
|
|
case 'y': // read stepper increment
|
|
// TODO
|
|
break;
|
|
case 'Y': // write stepper increment
|
|
// TODO
|
|
break;
|
|
case 'u': // read stepper threshold
|
|
// TODO
|
|
break;
|
|
case 'U': // write stepper threshold
|
|
// TODO
|
|
break;
|
|
case 'i': // set stepper direction to clockwise
|
|
stepper.dir = DIR_CLOCKWISE;
|
|
usart_send_blocking(USART2, 'i');
|
|
break;
|
|
case 'I': // set stepper direction to counterclockwise
|
|
stepper.dir = DIR_COUNTERCLOCKWISE;
|
|
usart_send_blocking(USART2, 'I');
|
|
break;
|
|
case 'o': // write brushed counter
|
|
// TODO
|
|
break;
|
|
case 'O': // read brushed counter
|
|
// TODO
|
|
break;
|
|
case 'p': // set brushed direction to clockwise
|
|
brushed.dir = DIR_CLOCKWISE;
|
|
usart_send_blocking(USART2, 'p');
|
|
break;
|
|
case 'P': // set brushed direction to counterclockwise
|
|
brushed.dir = DIR_COUNTERCLOCKWISE;
|
|
usart_send_blocking(USART2, 'P');
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
const struct {
|
|
bool active;
|
|
bool positive;
|
|
} phase_lut[][2] = {
|
|
{{true, true}, {false, true}}, // A+
|
|
//{{true, true}, {true, true}}, // A+, B+
|
|
{{false, true}, {true, true}}, // B+
|
|
//{{true, false}, {true, true}}, // A-, B+
|
|
{{true, false}, {false, true}}, // A-
|
|
//{{true, false}, {true, false}}, // A-, B-
|
|
{{false, false}, {true, false}}, // B-
|
|
//{{true, true}, {true, false}}, // A+, B-
|
|
};
|
|
|
|
|
|
void sys_tick_handler(void);
|
|
void sys_tick_handler(void) {
|
|
if (!stepper.active) {
|
|
gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
|
|
gpio_clear(GPIOB, GPIO1);
|
|
} else {
|
|
stepper.counter += stepper.increment;
|
|
if (stepper.counter >= stepper.threshold) {
|
|
--stepper.step_count;
|
|
stepper.counter -= stepper.threshold;
|
|
if (stepper.dir == DIR_CLOCKWISE) {
|
|
++stepper.phase;
|
|
} else {
|
|
--stepper.phase;
|
|
}
|
|
if (stepper.phase < 0) {
|
|
stepper.phase = 3;
|
|
}
|
|
if (stepper.phase >= 4) {
|
|
stepper.phase = 0;
|
|
}
|
|
}
|
|
if (stepper.step_count <= 0) {
|
|
stepper.counter = 0;
|
|
stepper.active = false;
|
|
stepper.done = true;
|
|
// power down stepper motor; TODO maybe lock its position?
|
|
gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
|
|
gpio_clear(GPIOB, GPIO1);
|
|
} else {
|
|
// write to GPIO pins
|
|
// TODO maybe use a set of OR/AND masks instead? this works but probably
|
|
// isn't constant time
|
|
if (phase_lut[stepper.phase][0].active) {
|
|
if (phase_lut[stepper.phase][0].positive) {
|
|
gpio_clear(GPIOA, GPIO6);
|
|
gpio_set(GPIOB, GPIO1);
|
|
} else {
|
|
gpio_set(GPIOA, GPIO6);
|
|
gpio_clear(GPIOB, GPIO1);
|
|
}
|
|
} else {
|
|
gpio_clear(GPIOA, GPIO6);
|
|
gpio_clear(GPIOB, GPIO1);
|
|
}
|
|
if (phase_lut[stepper.phase][1].active) {
|
|
if (phase_lut[stepper.phase][1].positive) {
|
|
gpio_clear(GPIOA, GPIO7);
|
|
gpio_set(GPIOA, GPIO5);
|
|
} else {
|
|
gpio_set(GPIOA, GPIO7);
|
|
gpio_clear(GPIOA, GPIO5);
|
|
}
|
|
} else {
|
|
gpio_clear(GPIOA, GPIO7);
|
|
gpio_clear(GPIOA, GPIO5);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (brushed.active) {
|
|
if (brushed.count == 0) {
|
|
// TODO toggle pins
|
|
}
|
|
++brushed.count;
|
|
if (brushed.count >= brushed.tocount) {
|
|
// TODO toggle pins
|
|
brushed.count = 0;
|
|
brushed.active = false;
|
|
brushed.done = true;
|
|
}
|
|
}
|
|
|
|
// TODO check limit switches
|
|
}
|