allow timings to be changed

Author: irishpatrick

include/pico_servo.h

@@ -26,12 +26,14 @@
 
 #include "pico/stdlib.h"
 
+void servo_set_bounds(uint, uint);
 int servo_init();
 int servo_clock_auto();
 int servo_clock_source(uint);
 int servo_clock_manual(uint);
 int servo_attach(uint);
 int servo_enable();
 int servo_move_to(uint, uint);
+int servo_microseconds(uint, uint);
 
-#endif /* PICO_SERVO_H */
+#endif /* PICO_SERVO_H */

src/pico_servo.c

@@ -31,20 +31,25 @@
 #include "hardware/structs/clocks.h"
 #include <string.h>
 
-
+#define KILO 1e3
+#define MICRO 1e-6
 #define WRAP 10000
 #define FREQ 50
 
-float clkdiv;
-uint min;
-uint max;
+static float clkdiv;
+static uint min;
+static uint max;
 
 static int slice_map[30];
 static uint slice_active[8];
 static uint servo_pos[32];
 static uint servo_pos_buf[16];
 static pwm_config slice_cfg[8];
 
+static uint min_us = 500;
+static uint max_us = 2500;
+static float us_per_unit = 0.f;
+
 static void wrap_cb()
 {
     uint offset;
@@ -62,6 +67,17 @@ static void wrap_cb()
     }
 }
 
+void servo_set_bounds(uint a, uint b)
+{
+    min_us = a;
+    max_us = b;
+    if (us_per_unit > 0.f)
+    {
+        min = min_us / us_per_unit;
+	max = max_us / us_per_unit;
+    }
+}
+
 int servo_init()
 {
     for (int i = 0; i < 30; ++i)
@@ -84,13 +100,18 @@ int servo_clock_auto()
 
 int servo_clock_source(uint src)
 {
-    clkdiv = (float)frequency_count_khz(src) * 1000.f / (FREQ * WRAP);
+    clkdiv = (float)frequency_count_khz(src) * (float)KILO / (FREQ * WRAP);
     if (clkdiv == 0)
     {
         return 1;
     }
-    min = 0.025f * WRAP;
-    max = 0.125f * WRAP;
+    us_per_unit = 1.f / (FREQ * WRAP) / MICRO;
+    
+    //min = 0.025f * WRAP;
+    //max = 0.125f * WRAP;
+
+    min = min_us / us_per_unit;
+    max = max_us / us_per_unit;
 
     return 0;
 }
@@ -150,4 +171,17 @@ int servo_move_to(uint pin, uint angle)
     servo_pos[16 * servo_pos_buf[pos] + pos] = val;
     servo_pos_buf[pos] = (servo_pos_buf[pos] + 1) % 2;
     return 0;
-}
+}
+
+int servo_microseconds(uint pin, uint us)
+{
+    if (slice_map[pin] < 0)
+    {
+        return 1;
+    }
+    
+    uint pos = slice_map[pin] + (pin % 2);
+    servo_pos[16 * servo_pos_buf[pos] + pos] = us;
+    servo_pos_buf[pos] = (servo_pos_buf[pos] + 1) % 2;
+    return 0;
+}