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);
+    }
+}