diff --git a/CyphalRobotController07-CAN-firmware.ino b/CyphalRobotController07-CAN-firmware.ino index 91206a9..e150a4b 100644 --- a/CyphalRobotController07-CAN-firmware.ino +++ b/CyphalRobotController07-CAN-firmware.ino @@ -34,6 +34,8 @@ //#define DBG_ENABLE_VERBOSE #include <107-Arduino-Debug.hpp> +#include "src/NeoPixelControl.h" + /************************************************************************************** * NAMESPACE **************************************************************************************/ @@ -59,12 +61,15 @@ static int const MOTOR1_2 = 6; static int const MOTOR1_EN = 10; static int const MOTOR0_EN = 11; static int const EM_STOP_PIN = 12; +static int const NEOPIXEL_PIN = 13; static int const OUTPUT_0_PIN = 21; /* GP21 */ static int const OUTPUT_1_PIN = 22; /* GP22 */ static int const ANALOG_INPUT_0_PIN = 26; static int const ANALOG_INPUT_1_PIN = 27; static int const ANALOG_INPUT_2_PIN = 28; +static int const NEOPIXEL_NUM_PIXELS = 12; /* Popular NeoPixel ring size */ + static SPISettings const MCP2515x_SPI_SETTING{10*1000*1000UL, MSBFIRST, SPI_MODE0}; static uint16_t const UPDATE_PERIOD_HEARTBEAT_ms = 1000; @@ -87,8 +92,8 @@ bool TimerHandler0(struct repeating_timer *t); Ifx007t mot0; Ifx007t mot1; -PioEncoder encoder0(ENCODER0_A); -PioEncoder encoder1(ENCODER1_A); +PioEncoder encoder0(ENCODER0_A, pio1); +PioEncoder encoder1(ENCODER1_A, pio1); INA226_WE ina226 = INA226_WE(); ADS1115_WE ads1115 = ADS1115_WE(); RPI_PICO_Timer ITimer0(0); @@ -135,9 +140,12 @@ cyphal::Publisher encoder1_pub; cyphal::Subscription output_0_subscription, output_1_subscription; cyphal::Subscription motor_0_subscription, motor_1_subscription; cyphal::Subscription motor_0_rpm_subscription, motor_1_rpm_subscription; +cyphal::Subscription light_mode_subscription; cyphal::ServiceServer execute_command_srv = node_hdl.create_service_server(2*1000*1000UL, onExecuteCommand_1_1_Request_Received); +NeoPixelControl neo_pixel_ctrl(NEOPIXEL_PIN, NEOPIXEL_NUM_PIXELS); + /* LITTLEFS/EEPROM ********************************************************************/ static EEPROM_24LCxx eeprom(EEPROM_24LCxx_Type::LC64, @@ -210,6 +218,7 @@ static CanardPortID port_id_motor0_bemf = std::numeric_limits::max(); static CanardPortID port_id_encoder0 = std::numeric_limits::max(); static CanardPortID port_id_encoder1 = std::numeric_limits::max(); +static CanardPortID port_id_light_mode = std::numeric_limits::max(); static uint16_t update_period_ms_internaltemperature = 10*1000; static uint16_t update_period_ms_input_voltage = 1*1000; @@ -288,6 +297,8 @@ const auto reg_rw_cyphal_sub_motor0_rpm_id = node_registry->exp const auto reg_ro_cyphal_sub_motor0_rpm_type = node_registry->route ("cyphal.sub.motor0rpm.type", {true}, []() { return "uavcan.primitive.scalar.Real32.1.0"; }); const auto reg_rw_cyphal_sub_motor1_rpm_id = node_registry->expose("cyphal.sub.motor1rpm.id", {true}, port_id_motor1_rpm); const auto reg_ro_cyphal_sub_motor1_rpm_type = node_registry->route ("cyphal.sub.motor1rpm.type", {true}, []() { return "uavcan.primitive.scalar.Real32.1.0"; }); +const auto reg_rw_cyphal_sub_lightmode_id = node_registry->expose("cyphal.sub.lightmode.id", {true}, port_id_light_mode); +const auto reg_ro_cyphal_sub_lightmode_type = node_registry->route ("cyphal.sub.lightmode.type", {true}, []() { return "uavcan.primitive.scalar.Integer8.1.0"; }); const auto reg_rw_crc07_update_period_ms_internaltemperature = node_registry->expose("crc07.update_period_ms.internaltemperature", {true}, update_period_ms_internaltemperature); const auto reg_rw_crc07_update_period_ms_input_voltage = node_registry->expose("crc07.update_period_ms.inputvoltage", {true}, update_period_ms_input_voltage); const auto reg_rw_crc07_update_period_ms_input_current = node_registry->expose("crc07.update_period_ms.inputcurrent", {true}, update_period_ms_input_current); @@ -362,19 +373,7 @@ void setup() node_id = 0; node_hdl.setNodeId(static_cast(node_id)); - if (port_id_internal_temperature != std::numeric_limits::max()) - internal_temperature_pub = node_hdl.create_publisher(port_id_internal_temperature, 1*1000*1000UL /* = 1 sec in usecs. */); - if (port_id_input_voltage != std::numeric_limits::max()) - input_voltage_pub = node_hdl.create_publisher(port_id_input_voltage, 1*1000*1000UL /* = 1 sec in usecs. */); - if (port_id_input_current != std::numeric_limits::max()) - input_current_pub = node_hdl.create_publisher(port_id_input_current, 1*1000*1000UL /* = 1 sec in usecs. */); - if (port_id_input_power != std::numeric_limits::max()) - input_power_pub = node_hdl.create_publisher(port_id_input_power, 1*1000*1000UL /* = 1 sec in usecs. */); - if (port_id_input_current_total != std::numeric_limits::max()) - input_current_total_pub = node_hdl.create_publisher(port_id_input_current_total, 5*1000*1000UL /* = 5 sec in usecs. */); - if (port_id_input_power_total != std::numeric_limits::max()) - input_power_total_pub = node_hdl.create_publisher(port_id_input_power_total, 5*1000*1000UL /* = 5 sec in usecs. */); - + /* all Cyphal subscription functions */ if (port_id_output0 != std::numeric_limits::max()) output_0_subscription = node_hdl.create_subscription( port_id_output0, @@ -463,6 +462,32 @@ void setup() motor0_enabled_flag = 1; }); + if (port_id_light_mode != std::numeric_limits::max()) + light_mode_subscription = node_hdl.create_subscription( + port_id_light_mode, + [](uavcan::primitive::scalar::Integer8_1_0 const & msg) + { + if(msg.value == 1) neo_pixel_ctrl.light_red(); + else if(msg.value == 2) neo_pixel_ctrl.light_green(); + else if(msg.value == 3) neo_pixel_ctrl.light_blue(); + else if(msg.value == 4) neo_pixel_ctrl.light_white(); + else if(msg.value == 5) neo_pixel_ctrl.light_amber(); + else neo_pixel_ctrl.light_off(); + }); + + /* all Cyphal publish functions */ + if (port_id_internal_temperature != std::numeric_limits::max()) + internal_temperature_pub = node_hdl.create_publisher(port_id_internal_temperature, 1*1000*1000UL /* = 1 sec in usecs. */); + if (port_id_input_voltage != std::numeric_limits::max()) + input_voltage_pub = node_hdl.create_publisher(port_id_input_voltage, 1*1000*1000UL /* = 1 sec in usecs. */); + if (port_id_input_current != std::numeric_limits::max()) + input_current_pub = node_hdl.create_publisher(port_id_input_current, 1*1000*1000UL /* = 1 sec in usecs. */); + if (port_id_input_power != std::numeric_limits::max()) + input_power_pub = node_hdl.create_publisher(port_id_input_power, 1*1000*1000UL /* = 1 sec in usecs. */); + if (port_id_input_current_total != std::numeric_limits::max()) + input_current_total_pub = node_hdl.create_publisher(port_id_input_current_total, 5*1000*1000UL /* = 5 sec in usecs. */); + if (port_id_input_power_total != std::numeric_limits::max()) + input_power_total_pub = node_hdl.create_publisher(port_id_input_power_total, 5*1000*1000UL /* = 5 sec in usecs. */); if (port_id_em_stop != std::numeric_limits::max()) em_stop_pub = node_hdl.create_publisher(port_id_em_stop, 1*1000*1000UL /* = 1 sec in usecs. */); if (port_id_analog_input0 != std::numeric_limits::max()) @@ -564,6 +589,14 @@ void setup() else DBG_ERROR("Can't set ITimer0. Select another Timer, freq. or timer"); + /* enable neopixels */ + neo_pixel_ctrl.begin(); + + neo_pixel_ctrl.light_red(); + delay(100); + neo_pixel_ctrl.light_amber(); + delay(100); + neo_pixel_ctrl.light_green(); /* configure INA226, current sensor, set conversion time and average to get a value every two seconds */ ina226.init(); diff --git a/src/NeoPixelControl.cpp b/src/NeoPixelControl.cpp new file mode 100644 index 0000000..c933a77 --- /dev/null +++ b/src/NeoPixelControl.cpp @@ -0,0 +1,64 @@ +/* + * Software for the auxiliary controller for the L3X-Z Hexapod + */ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include "NeoPixelControl.h" + +/************************************************************************************** + * CTOR/DTOR + **************************************************************************************/ + +NeoPixelControl::NeoPixelControl(int const pin, int const num_pixels) +: _pixels(num_pixels, pin, NEO_GRB) +{ + +} + +/************************************************************************************** + * PUBLIC MEMBER FUNCTIONS + **************************************************************************************/ + +void NeoPixelControl::begin() +{ + _pixels.begin(); +} + +void NeoPixelControl::light_off() +{ + _pixels.clear(); + _pixels.show(); +} + +void NeoPixelControl::light_green() +{ + _pixels.fill(_pixels.Color(0, 55, 0)); + _pixels.show(); +} + +void NeoPixelControl::light_red() +{ + _pixels.fill(_pixels.Color(55, 0, 0)); + _pixels.show(); +} + +void NeoPixelControl::light_blue() +{ + _pixels.fill(_pixels.Color(0, 0, 55)); + _pixels.show(); +} + +void NeoPixelControl::light_white() +{ + _pixels.fill(_pixels.Color(55, 55, 55)); + _pixels.show(); +} + +void NeoPixelControl::light_amber() +{ + _pixels.fill(_pixels.Color(55, 40, 0)); + _pixels.show(); +} diff --git a/src/NeoPixelControl.h b/src/NeoPixelControl.h new file mode 100644 index 0000000..71c2c62 --- /dev/null +++ b/src/NeoPixelControl.h @@ -0,0 +1,42 @@ +/* + * Software for the auxiliary controller for the L3X-Z Hexapod + */ + +#ifndef AUX_CTRL_NEO_PIXEL_CONTROL_H_ +#define AUX_CTRL_NEO_PIXEL_CONTROL_H_ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +/************************************************************************************** + * CLASS DEFINITION + **************************************************************************************/ + +class NeoPixelControl +{ +public: + + NeoPixelControl(int const pin, int const num_pixels); + + + void begin(); + + + void light_off(); + void light_green(); + void light_red(); + void light_blue(); + void light_white(); + void light_amber(); + + inline Adafruit_NeoPixel & pixels() { return _pixels; } + + +private: + Adafruit_NeoPixel _pixels; +}; + +#endif /* AUX_CTRL_NEO_PIXEL_CONTROL_H_ */