Glorious basic motor code

src/main.c

@@ -110,6 +110,23 @@ const uint8_t better_matrix[65][17] = {
     {0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0b11111111, 0x00},
 };
 
+void motor_forward()
+{
+    gpio_put(20, 1);
+    gpio_put(21, 0);
+}
+
+void motor_backward()
+{
+    gpio_put(20, 0);
+    gpio_put(21, 1);
+}
+
+void set_speed(double speed)
+{
+    volatile uint slice_num = pwm_gpio_to_slice_num(19);
+    pwm_set_chan_level(slice_num, PWM_CHAN_B, speed);
+}
 void set_progress(double percentage)
 {
 
@@ -119,7 +136,7 @@ void set_progress(double percentage)
 
     // for (int i = 0; i <= numerator; i++)
     //{
-    //   i2c_write_blocking(i2c_default, 0x70, better_matrix[i], 17, false);
+    //    i2c_write_blocking(i2c_default, 0x70, better_matrix[i], 17, false);
     //}
 }
 
@@ -159,6 +176,22 @@ void callback(uint gpio, uint32_t events)
             flipflop = true;
         }
 
+        int avg = (percentage + percentage_1 + percentage_2) / 3;
+        // if zwischen 90, 110, stop
+
+        if (avg > 90 && avg < 110)
+        {
+            gpio_put(20, 0);
+            gpio_put(21, 0);
+        }
+        else if (avg < 90)
+        {
+            motor_forward();
+        }
+        else if (avg > 110)
+        {
+            motor_backward();
+        }
         set_progress((percentage + percentage_1 + percentage_2) / 3);
     }
 
@@ -182,6 +215,7 @@ void init_pwm()
     pwm_set_chan_level(slice_num, PWM_CHAN_A, 10);
     pwm_set_enabled(slice_num, true);
 }
+
 void pwm_echo_init()
 {
     gpio_init(27);
@@ -203,7 +237,31 @@ void pwm_echo_init()
     pwm_set_clkdiv_mode(slice_num, PWM_DIV_FREE_RUNNING);
     pwm_set_enabled(slice_num, true);
 }
-int a = 0;
+
+void init_motor()
+{
+    gpio_init(19);
+    gpio_set_dir(19, GPIO_OUT);
+
+    gpio_set_function(19, GPIO_FUNC_PWM);
+
+    gpio_init(20);
+    gpio_set_dir(20, GPIO_OUT);
+
+    gpio_init(21);
+    gpio_set_dir(21, GPIO_OUT);
+
+    volatile uint slice_num = pwm_gpio_to_slice_num(19);
+
+    pwm_set_clkdiv(slice_num, 100);                  // 100 -> 1.25 Mhz
+    pwm_set_wrap(slice_num, 10000);                  // 10000 -> 10k * 1mhz = 10k microseconds = 10 ms total
+    pwm_set_chan_level(slice_num, PWM_CHAN_B, 2500); // 2000 - 10000 = HIGH FOR 3 MILLISECONDS
+    // 3 High, 7 low, 3 High
+
+    // 50 Hz für langsam, 100 für schnell
+    pwm_set_enabled(slice_num, true);
+    // gpio_put(19, 1);
+}
 
 int main()
 {
@@ -229,6 +287,7 @@ int main()
 
     init_pwm();
     pwm_echo_init();
+    init_motor();
 
     gpio_set_irq_enabled_with_callback(27, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, callback);
 
@@ -268,6 +327,5 @@ int main()
     while (1)
     {
     }
-    a = 3;
     return 0;
 }
\ No newline at end of file