Merge pull request #38 from LexxPluss/HWDEV-2176-add-tug-encoder-cont…

…roller

HWDEV-2176 add new tug encoder support

lexxpluss_apps/prj.conf

@@ -50,4 +50,5 @@ CONFIG_WDT_DISABLE_AT_BOOT=n
 CONFIG_MAXBOTIX=y
 CONFIG_ADIS16470=n
 CONFIG_IIM42652=y
+CONFIG_I2C=y
 

lexxpluss_apps/src/board_controller.cpp

@@ -1287,7 +1287,7 @@ class state_controller { // Variables Implemented
         ac.poll();
         bmu.poll();
         mbd.poll();
-        
+
         switch (state) {
         case POWER_STATE::OFF:
             if (ksw.is_maintenance()) {

lexxpluss_apps/src/main.cpp

@@ -38,6 +38,7 @@
 #include "runaway_detector.hpp"
 #include "uss_controller.hpp"
 #include "gpio_controller.hpp"
+#include "tug_encoder_controller.hpp"
 
 namespace {
 
@@ -53,6 +54,7 @@ K_THREAD_STACK_DEFINE(pgv_controller_stack, 2048);
 K_THREAD_STACK_DEFINE(runaway_detector_stack, 2048);
 K_THREAD_STACK_DEFINE(uss_controller_stack, 2048);
 K_THREAD_STACK_DEFINE(gpio_controller_stack, 2048);
+K_THREAD_STACK_DEFINE(tug_encoder_controller_stack, 2048);
 K_THREAD_STACK_DEFINE(zcan_main_stack, 2048);
 
 #define RUN(name, prio) \
@@ -270,6 +272,7 @@ int main()
     lexxhard::runaway_detector::init();
     lexxhard::uss_controller::init();
     lexxhard::gpio_controller::init();
+    lexxhard::tug_encoder_controller::init();
 
     RUN(actuator_controller, 2);
     RUN(adc_reader, 2);
@@ -282,6 +285,7 @@ int main()
     RUN(pgv_controller, 1);
     RUN(uss_controller, 2);
     RUN(gpio_controller, 2);
+    RUN(tug_encoder_controller, 2);
     RUN(runaway_detector, 4);
     RUN(zcan_main, 5); // zcan_main thread must be started at last.
 

lexxpluss_apps/src/tug_encoder_controller.cpp

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2024, LexxPluss Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/shell/shell.h>
+#include <zephyr/drivers/i2c.h>
+#include "tug_encoder_controller.hpp"
+
+namespace lexxhard::tug_encoder_controller { 
+
+LOG_MODULE_REGISTER(tug_encoder);
+
+char __aligned(4) msgq_buffer[8 * sizeof (msg)];
+
+class tug_encoder_controller_impl {
+public:
+    int init() {
+        k_msgq_init(&msgq, msgq_buffer, sizeof (msg), 8);
+
+        dev = DEVICE_DT_GET(DT_NODELABEL(i2c2));
+        if (!device_is_ready(dev)) {
+            LOG_ERR("TUG Encoder device not found");
+            return -1;
+        }
+
+        return 0;
+    }
+
+    void run() {
+        if (!device_is_ready(dev)) {
+            LOG_ERR("TUG Encoder device not found");
+            return;
+        }
+
+        while (true) {
+            fetch_angle();
+            while (k_msgq_put(&msgq, &message, K_NO_WAIT) != 0)
+                k_msgq_purge(&msgq);
+            k_msleep(1);
+        }
+    }
+
+    void tug_encoder_info(const shell *shell) const {
+        float const angle_deg = message.angle * 360.0f / 4096.0f;
+        shell_print(shell, "Angle: %f[deg]\n", angle_deg);
+    }
+
+private:
+    void fetch_angle() {
+        uint8_t angle_h;
+        if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ANGLE_H, &angle_h)) {
+            LOG_ERR("Failed to read angle high byte\n");
+            return;
+        }
+        uint8_t angle_l;
+        if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ANGLE_L, &angle_l)) {
+            LOG_ERR("Failed to read angle low byte\n");
+            return;
+        }
+
+        message.angle = (static_cast<uint16_t>(angle_h) << 8) | angle_l;
+    }
+    msg message;
+    const device *dev{nullptr};
+
+    static constexpr uint8_t AS5600_ADDR{0x36};
+    static constexpr uint8_t AS5600_REG_ANGLE_H{0x0E};
+    static constexpr uint8_t AS5600_REG_ANGLE_L{0x0F};
+} impl;
+
+int tug_encoder_info(const shell *shell, size_t argc, char **argv)
+{
+    impl.tug_encoder_info(shell);
+    return 0;
+}
+
+SHELL_STATIC_SUBCMD_SET_CREATE(sub_tug_encoder,
+    SHELL_CMD(info, NULL, "TUG Encoder information", tug_encoder_info),
+    SHELL_SUBCMD_SET_END
+);
+SHELL_CMD_REGISTER(tug_encoder, &sub_tug_encoder, "TUG Encoder commands", NULL);
+
+void init()
+{
+    impl.init();
+}
+
+void run(void *p1, void *p2, void *p3)
+{
+    impl.run();
+}
+
+k_thread thread;
+k_msgq msgq;
+
+}
+

lexxpluss_apps/src/tug_encoder_controller.hpp

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2024, LexxPluss Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#pragma once
+
+#include <zephyr/kernel.h>
+
+namespace lexxhard::tug_encoder_controller {
+
+#define TUG_ENCODER_CAN_DATA_LENGTH 2
+
+struct msg {
+    uint16_t angle;
+} __attribute__((aligned(2)));
+
+void init();
+void run(void *p1, void *p2, void *p3);
+extern k_thread thread;
+extern k_msgq msgq;
+}
+

lexxpluss_apps/src/zcan_main.cpp

@@ -33,6 +33,7 @@
 #include "zcan_uss.hpp"
 #include "zcan_dfu.hpp"
 #include "zcan_gpio.hpp"
+#include "zcan_tug_encoder.hpp"
 #include "zcan_main.hpp"
 
 namespace lexxhard::zcan_main {
@@ -62,6 +63,7 @@ class {
         pgv.init();
         uss.init();
         gpio.init();
+        tug_encoder.init();
         return 0;
     }
     void run() {
@@ -75,6 +77,7 @@ class {
             pgv.poll();
             uss.poll();
             gpio.poll();
+            tug_encoder.poll();
             k_usleep(1);
         }
     }
@@ -89,6 +92,7 @@ class {
     lexxhard::zcan_pgv::zcan_pgv pgv;
     lexxhard::zcan_uss::zcan_uss uss;
     lexxhard::zcan_gpio::zcan_gpio gpio;
+    lexxhard::zcan_tug_encoder::zcan_tug_encoder tug_encoder;
 } impl;
 
 void init()

lexxpluss_apps/src/zcan_tug_encoder.hpp

@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2024, LexxPltug_encoder Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <zephyr/kernel.h>
+#include <zephyr/drivers/can.h>
+#include <zephyr/logging/log.h>
+#include "tug_encoder_controller.hpp"
+
+#define CAN_ID_TUG_ENCODER 0x210
+#define CAN_DATA_LENGTH_TUG_ENCODER 2
+
+namespace lexxhard::zcan_tug_encoder {
+
+LOG_MODULE_REGISTER(zcan_tug_encoder);
+
+class zcan_tug_encoder {
+public:
+    void init() {
+        dev = DEVICE_DT_GET(DT_NODELABEL(can2));
+        if (!device_is_ready(dev)){
+            LOG_ERR("CAN_2 is not ready");
+            return;
+        }
+    }
+    void poll() {
+        tug_encoder_controller::msg message;
+
+        while (k_msgq_get(&tug_encoder_controller::msgq, &message, K_NO_WAIT) == 0) {
+            uint8_t packedData[CAN_DATA_LENGTH_TUG_ENCODER] {0};
+            packedData[0] = message.angle >> 8;
+            packedData[1] = message.angle & 0xFF;
+
+            // set data to CAN frame
+            can_frame frame{
+                .id = CAN_ID_TUG_ENCODER,
+                .dlc = CAN_DATA_LENGTH_TUG_ENCODER,
+                .data = {0}
+            };
+
+            // copy packedData to CAN frame data
+            memcpy(frame.data, packedData, CAN_DATA_LENGTH_TUG_ENCODER);
+
+            can_send(dev, &frame, K_MSEC(100), nullptr, nullptr);
+        }
+    }
+private:
+    const device *dev{nullptr};
+};
+
+}
+