diff options
| -rw-r--r-- | Year_3/LSM/ex_20190218.c | 147 |
1 files changed, 147 insertions, 0 deletions
diff --git a/Year_3/LSM/ex_20190218.c b/Year_3/LSM/ex_20190218.c new file mode 100644 index 0000000..e43afd8 --- /dev/null +++ b/Year_3/LSM/ex_20190218.c @@ -0,0 +1,147 @@ +#include "stm32_unict_lib.h" +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> + +int tmax; +typedef enum { + SETUP, + RUN +} status_t; + +status_t status = RUN; +float real_root = 0; +float new_want_root = 0; +float want_root = 0; + +double rotation = 1; + +void +init() +{ + GPIO_init(GPIOB); + GPIO_init(GPIOC); + + GPIO_config_input(GPIOB, 10); /* X */ + GPIO_config_EXTI(GPIOB, EXTI10); + EXTI_enable(EXTI10, FALLING_EDGE); + + GPIO_config_input(GPIOB, 4); /* Y */ + GPIO_config_EXTI(GPIOB, EXTI4); + EXTI_enable(EXTI4, FALLING_EDGE); + + GPIO_config_output(GPIOB, 0); /* LED RED */ + GPIO_config_output(GPIOC, 3); /* LED GREEN */ + + TIM_init(TIM2); + TIM_set(TIM2, 0); + TIM_on(TIM2); + TIM_config_timebase(TIM2, 8400, 840); + TIM_enable_irq(TIM2, IRQ_UPDATE); + + ADC_init(ADC1, ADC_RES_12, ADC_ALIGN_RIGHT); + ADC_channel_config(ADC1, GPIOC, 1, 11); + ADC_channel_config(ADC1, GPIOC, 0, 10); + ADC_on(ADC1); + + DISPLAY_init(); + CONSOLE_init(); +} + +int +main() +{ + init(); + char s[5]; + srand(time(NULL)); + + for (;;) { + if (status == RUN) { + ADC_sample_channel(ADC1, 10); + ADC_start(ADC1); + while (!ADC_completed(ADC1)) + ; + float adc_read = ADC_read(ADC1); + + adc_read = (2 * adc_read) / 4095; + real_root -= ((adc_read * 2) * (1.0 * rand()) / RAND_MAX - adc_read); + + printf("%f %f %f\n", rotation, real_root, want_root); + sprintf(s, "%f", real_root); + + DISPLAY_puts(0, s); + delay_ms(500); + } else { + ADC_sample_channel(ADC1, 11); + ADC_start(ADC1); + while (!ADC_completed(ADC1)) + ; + float adc_read = ADC_read(ADC1); + new_want_root = (((180 + 180) * adc_read) / 4095) - 180; + sprintf(s, "%f", new_want_root); + + DISPLAY_puts(0, s); + delay_ms(500); + } + } + return 0; +} + +void +EXTI4_IRQHandler(void) +{ + if (EXTI_isset(EXTI4)) { + if (status == SETUP) { + status = RUN; + want_root = new_want_root; + GPIO_write(GPIOB, 0, 0); + } + EXTI_clear(EXTI4); + } +} + +void +EXTI15_10_IRQHandler(void) +{ + if (EXTI_isset(EXTI10)) { + if (status == RUN) { + status = SETUP; + GPIO_write(GPIOB, 0, 1); + } else { + status = RUN; + GPIO_write(GPIOB, 0, 0); + } + + EXTI_clear(EXTI10); + } +} + +void +TIM2_IRQHandler(void) +{ + if (TIM_update_check(TIM2)) { + rotation = (want_root - real_root) * 0.08; + + if (rotation < -10) { + rotation = -10; + } else if (rotation > 10) { + rotation = 10; + } + + real_root = real_root + rotation * 0.04; + if (real_root < -180) { + real_root = -180; + } else { + real_root = 180; + } + + if (abs(want_root - real_root) <= 2) { + GPIO_write(GPIOC, 3, 1); + } else { + GPIO_write(GPIOC, 3, 0); + } + + TIM_update_clear(TIM2); + } +} |