wip: stepper

src/devices.cpp

@@ -27,6 +27,14 @@ bool verify_are_devices_available(void) {
         printk("Counter 5 not ready\n");
         devices_ready = false;
     }
+    if (!device_is_ready(rotenc0)) {
+        printk("Rotory encoder 0 not ready\n");
+        devices_ready = false;
+    }
+    if (!device_is_ready(stepper0)) {
+        printk("Stepper 0 not ready\n");
+        devices_ready = false;
+    }
 
     return devices_ready;
 }

src/devices.hpp

@@ -7,6 +7,7 @@
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
 #include <zephyr/drivers/pwm.h>
+#include <zephyr/drivers/stepper.h>
 #else
 #include <device.h>
 #include <devicetree.h>
@@ -17,6 +18,8 @@ static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
 static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
 static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
 
+static const struct device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0));
+static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));
 static const struct pwm_dt_spec servo0 = PWM_DT_SPEC_GET(DT_ALIAS(servo0));
 static const struct device *counter5 = DEVICE_DT_GET(DT_NODELABEL(counter5));
 

src/servo_controller/servo_controller.cpp

@@ -1,9 +1,13 @@
 #include "servo_controller.hpp"
 #include "devices.hpp"
+#include "rcl/types.h"
 #include "rcl_util.hpp"
+#include "zephyr/sys/printk.h"
 
 #include <std_msgs/msg/float32.h>
 #include <zephyr/drivers/counter.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/drivers/stepper.h>
 #include <zephyr/kernel.h>
 #include <rmw_microros/rmw_microros.h>
 
@@ -57,6 +61,12 @@ void counter_cb(const struct device *dev, uint8_t chan_id, uint32_t ticks, void
 
 rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* executor)
 {
+    uint32_t speed_hz = 6400;  // desired step frequency in Hz
+    uint32_t interval_ns = 1000000000U / speed_hz;  // ns per step
+    stepper_set_microstep_interval(stepper0, interval_ns);
+    stepper_enable(stepper0);
+    stepper_run(stepper0, STEPPER_DIRECTION_POSITIVE);
+
     RCL_RETURN_ON_ERROR(rclc_node_init_default(
         &ServoController::node, 
         "servo_controller",
@@ -164,8 +174,21 @@ void ServoController::servo_pid() {
 rcl_ret_t ServoController::publish() {
     std_msgs__msg__Float32 msg;
     std_msgs__msg__Float32__init(&msg);
+    struct sensor_value val;
 
-    msg.data = ServoController::current_angle;
+    if (sensor_sample_fetch(rotenc0)) {
+        printk("failed to fetch sensor value\n");
+        return RCL_RET_OK;
+    }
+    
+    if (sensor_channel_get(rotenc0, SENSOR_CHAN_ROTATION, &val)) {
+        printk("failed to get sensor value\n");
+        return RCL_RET_OK;
+    }   
+    printk("angle: %d.%d\n", val.val1, val.val2);
+
+    //msg.data = ServoController::current_angle;
+    msg.data = (float)val.val1 + (float)val.val1 / 1000000;
 
     return rcl_publish(&ServoController::servo0_publisher, &msg, NULL);
 }