From f9b7da600faed60fd45fc028ded0563797203bff Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 18 Jun 2024 02:09:16 -0700 Subject: [PATCH 01/12] Portenta C33 version stands up for a while but still falls over --- .clang-format | 15 +++- .gitignore | 1 + .vscode/arduino.json | 6 -- BalanceBot.ino | 73 ++++++------------ src/SparkFun_BNO080_Arduino_Library.h | 2 + src/WiFI_server.hpp | 105 -------------------------- src/balancer.hpp | 4 +- src/bot_can.hpp | 72 ++++++++++++------ src/imu_wrapper.hpp | 13 ++-- src/portenta_rgb.hpp | 19 +++++ 10 files changed, 118 insertions(+), 192 deletions(-) delete mode 100644 .vscode/arduino.json delete mode 100644 src/WiFI_server.hpp create mode 100644 src/portenta_rgb.hpp diff --git a/.clang-format b/.clang-format index afbd719..feb7542 100644 --- a/.clang-format +++ b/.clang-format @@ -4,5 +4,18 @@ TabWidth: 4 IndentWidth: 4 ColumnLimit: 0 AlignConsecutiveAssignments: Consecutive -AllowShortCaseLabelsOnASingleLine: true AlignConsecutiveDeclarations: Consecutive + +AllowShortBlocksOnASingleLine: Always +AllowShortCaseExpressionOnASingleLine: true +AllowShortCaseLabelsOnASingleLine: true +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: true + +AlignAfterOpenBracket: BlockIndent +BinPackArguments: false +BinPackParameters: false \ No newline at end of file diff --git a/.gitignore b/.gitignore index da5e10f..fc67c02 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ build/ .vscode/c_cpp_properties.json WiFi_secrets.hpp +arduino.json \ No newline at end of file diff --git a/.vscode/arduino.json b/.vscode/arduino.json deleted file mode 100644 index 1976bc1..0000000 --- a/.vscode/arduino.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "board": "arduino:samd:mkrwifi1010", - "sketch": "BalanceBot.ino", - "output": "build/", - "port": "COM3" -} \ No newline at end of file diff --git a/BalanceBot.ino b/BalanceBot.ino index 454b15d..58a5987 100644 --- a/BalanceBot.ino +++ b/BalanceBot.ino @@ -1,32 +1,32 @@ -#include +#include +#include #include -#include "src/MKRrgb.hpp" #include "src/ODriveEnums.h" -#include "src/WiFI_server.hpp" #include "src/balancer.hpp" #include "src/bot_can.hpp" #include "src/can_helpers.hpp" #include "src/can_simple_messages.hpp" #include "src/config.hpp" #include "src/imu_wrapper.hpp" +#include "src/portenta_rgb.hpp" #include "src/utils.hpp" +#ifdef ARDUINO_PORTENTA_C33 +// TODO: Make it easy to configure this +#endif + // Global object initialization BotCanClass bot_can; BotController controller; ImuWrapper imu; -MKRrgb pixel; +RgbC33 pixel; // Task handles -static TaskHandle_t imu_task; -static TaskHandle_t can_task; -static TaskHandle_t control_task; -static TaskHandle_t wifi_task; - -static WiFiServer server{80}; +static TaskHandle_t taskHandle_1kHz; +static TaskHandle_t taskHandle_100Hz; void setup() { configControllers(); @@ -39,14 +39,15 @@ void setup() { delay(1); } - // Initialize MKR RGB LED + // Init objects pixel.setup(); + imu.begin(Wire2); + bot_can.setup(); + controller.begin(); // Create RTOS tasks - xTaskCreate(controlTask, "control Task", 256, nullptr, tskIDLE_PRIORITY + 4, &control_task); - xTaskCreate(canTask, "CAN Task", 256, nullptr, tskIDLE_PRIORITY + 3, &can_task); - xTaskCreate(imuTask, "IMU Task", 256, nullptr, tskIDLE_PRIORITY + 2, &imu_task); - xTaskCreate(wifiTask, "wifi Task", 256, nullptr, tskIDLE_PRIORITY + 1, &wifi_task); + xTaskCreate(periodic_1kHz, "IMU Task", 256, nullptr, tskIDLE_PRIORITY + 3, &taskHandle_1kHz); + xTaskCreate(periodic_100Hz, "CAN Task", 256, nullptr, tskIDLE_PRIORITY + 2, &taskHandle_100Hz); // Start RTOS tasks Serial.println("Starting Scheduler"); @@ -61,61 +62,31 @@ void setup() { } } -// RTOS Idle Loop -void loop() { - delay(1000); -} - -static void controlTask(void *pvParameters) { +static void periodic_100Hz(void *pvParameters) { TickType_t lastWakeTime = xTaskGetTickCount(); - // Initialize controller - controller.begin(); - // Run this code periodically at 100Hz for (;;) { vTaskDelayUntil(&lastWakeTime, 10UL); controller.step(); - } -} - -static void canTask(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - - // Initialize CAN bus - bot_can.setup(); - - // Run this code periodically at 100Hz - for (;;) { - vTaskDelayUntil(&lastWakeTime, 10UL); - - bot_can.read(); bot_can.send(); } } -static void imuTask(void *pvParameters) { +static void periodic_1kHz(void *pvParameters) { TickType_t lastWakeTime = xTaskGetTickCount(); - // Start the IMU on i2c, at 400kHz - imu.begin(); - // Run this code periodically at 1kHz for (;;) { vTaskDelayUntil(&lastWakeTime, 1UL); imu.read(); + bot_can.read(); } } -static void wifiTask(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - - Wifisetup(server); - - // Run this code periodically - for (;;) { - Wifiloop(server); - } +// RTOS Idle Loop +void loop() { + delay(1000); } diff --git a/src/SparkFun_BNO080_Arduino_Library.h b/src/SparkFun_BNO080_Arduino_Library.h index 37269f9..581adef 100644 --- a/src/SparkFun_BNO080_Arduino_Library.h +++ b/src/SparkFun_BNO080_Arduino_Library.h @@ -48,7 +48,9 @@ #else // The catch-all default is 32 +#ifndef I2C_BUFFER_LENGTH #define I2C_BUFFER_LENGTH 32 +#endif #endif //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= diff --git a/src/WiFI_server.hpp b/src/WiFI_server.hpp deleted file mode 100644 index 8b77ac6..0000000 --- a/src/WiFI_server.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "WiFi_secrets.hpp" - -void printWiFiStatus() { - // print the SSID of the network you're attached to: - Serial.print("SSID: "); - Serial.println(WiFi.SSID()); - - // print your WiFi shield's IP address: - IPAddress ip = WiFi.localIP(); - Serial.print("IP Address: "); - Serial.println(ip); -} - -void Wifisetup(WiFiServer& server) { - // check for the WiFi module: - if (WiFi.status() == WL_NO_MODULE) { - Serial.println("Communication with WiFi module failed!"); - // don't continue - while (true) - ; - } - - String fv = WiFi.firmwareVersion(); - if (fv < WIFI_FIRMWARE_LATEST_VERSION) { - Serial.println("Please upgrade the firmware"); - } - - // attempt to connect to WiFi network: - Serial.print("Attempting to connect to SSID: "); - Serial.println(ssid); - - // Connect to WPA/WPA2 network. Change this line if using open or WEP network: - int status = WiFi.begin(ssid, pass); - - while (status != WL_CONNECTED) { - // wait for connection: - vTaskDelay(1000); - } - - server.begin(); - - // you're connected now, so print out the status: - Serial.print("Connected to "); - printWiFiStatus(); -} - -void Wifiloop(WiFiServer& server) { - // listen for incoming clients - WiFiClient client = server.available(); - if (client) { - Serial.println("new client"); - // an HTTP request ends with a blank line - boolean currentLineIsBlank = true; - while (client.connected()) { - if (client.available()) { - char c = client.read(); - Serial.write(c); - // if you've gotten to the end of the line (received a newline - // character) and the line is blank, the HTTP request has ended, - // so you can send a reply - if (c == '\n' && currentLineIsBlank) { - // send a standard HTTP response header - client.println("HTTP/1.1 200 OK"); - client.println("Content-Type: text/html"); - client.println("Connection: close"); // the connection will be closed after completion of the response - client.println("Refresh: 5"); // refresh the page automatically every 5 sec - client.println(); - client.println(""); - client.println(""); - // output the value of each analog input pin - for (int analogChannel = 0; analogChannel < 6; analogChannel++) { - int sensorReading = analogRead(analogChannel); - client.print("analog input "); - client.print(analogChannel); - client.print(" is "); - client.print(sensorReading); - client.println("
"); - } - client.println(""); - break; - } - if (c == '\n') { - // you're starting a new line - currentLineIsBlank = true; - } else if (c != '\r') { - // you've gotten a character on the current line - currentLineIsBlank = false; - } - } - } - - // give the web browser time to receive the data - vTaskDelay(1); - - // close the connection: - client.stop(); - Serial.println("client disconnected"); - } -} diff --git a/src/balancer.hpp b/src/balancer.hpp index 874898f..b078e07 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -91,8 +91,8 @@ struct BotController { const float drive_cmd = 0.0f; // rx.drive * (1.0f - abs(steer_cmd)); // Motor speeds - const float vel_right = bot_can.right_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi); // [m/s] right wheel speed - const float vel_left = -1.0f * bot_can.left_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi); // [m/s] left wheel speed + const float vel_right = +1.0f * bot_can.right_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi); // [m/s] right wheel speed + const float vel_left = -1.0f * bot_can.left_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi); // [m/s] left wheel speed // TODO: Verify yaw rate calculation matches gyro reading const float vel_actual = (vel_right + vel_left) / 2.0f + (d2r(imu.pitch_rate) * settings.kComHeight); // [m/s] Vehicle speed diff --git a/src/bot_can.hpp b/src/bot_can.hpp index b2e8d89..a985754 100644 --- a/src/bot_can.hpp +++ b/src/bot_can.hpp @@ -1,26 +1,44 @@ #pragma once -// Adafruit Feather M4 CAN -#include -#include +#include #include "ODriveEnums.h" #include "can_simple_messages.hpp" // Helper function for sending can messages void sendCanMsg(const can_Message_t &msg) { - CAN.beginPacket(msg.id, msg.len); - CAN.write(msg.data, msg.len); - CAN.endPacket(); + const CanMsg c33msg{CanStandardId(msg.id), msg.len, msg.data}; + + auto ret = CAN1.write(c33msg); + + if (ret != 1) { + Serial.print("Tx Failed: "); + Serial.print(ret, HEX); + Serial.print(" - "); + Serial.println(c33msg); + } else { + Serial.println("Tx Success"); + } + + // TODO: Figure out why this is needed - no FIFO in R7FA6M5_CAN ??? + delayMicroseconds(500); } struct BotCanClass { void setup() { +// Portenta C33 FD transceiver pins +#ifdef ARDUINO_PORTENTA_C33 + pinMode(PIN_CAN1_STBY, OUTPUT); + digitalWrite(PIN_CAN1_STBY, LOW); +#endif + left_motor.axis_id_ = 0; right_motor.axis_id_ = 1; // Start CAN at 500kbps - if (!CAN.begin(500000)) { + CAN1.disableInternalLoopback(); + + if (!CAN1.begin(CanBitRate::BR_500k)) { Serial.println("CAN Begin Failed!"); } else { Serial.println("Connected to CAN at 500kbps"); @@ -34,27 +52,24 @@ struct BotCanClass { left_motor.set_axis_state_msg.Axis_Requested_State = state; right_motor.set_axis_state_msg.Axis_Requested_State = state; - sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetControllerModeMsg)); - sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetControllerModeMsg)); - - sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetLimitsMsg)); - sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetLimitsMsg)); - - sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetAxisStateMsg)); - sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetAxisStateMsg)); + m_axis_state_update = true; } void read() { - while (CAN.parsePacket()) { - can_Message_t rxmsg; + while (CAN1.available()) { + const CanMsg rxmsg = CAN1.read(); - rxmsg.id = CAN.packetId(); - CAN.readBytes(rxmsg.data, 8); + can_Message_t odrv_msg = { + .id = rxmsg.id, + .len = rxmsg.data_length + }; + + std::memcpy(odrv_msg.data, rxmsg.data, rxmsg.data_length); // pixel.setColor(0, led, led); switch (ODriveArduinoCAN::get_node_id(rxmsg.id)) { - case 0: left_motor.decode(rxmsg); break; - case 1: right_motor.decode(rxmsg); break; + case 0: left_motor.decode(odrv_msg); break; + case 1: right_motor.decode(odrv_msg); break; default: break; } } @@ -64,11 +79,26 @@ struct BotCanClass { // Send the periodic CAN messages sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); + + if (m_axis_state_update) { + sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetControllerModeMsg)); + sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetControllerModeMsg)); + + sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetLimitsMsg)); + sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetLimitsMsg)); + + sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetAxisStateMsg)); + sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetAxisStateMsg)); + + m_axis_state_update = false; + } } // CAN communication objects for the ODrives ODriveArduinoCAN left_motor; // Node ID 0 ODriveArduinoCAN right_motor; // Node ID 1 + + bool m_axis_state_update = false; }; extern BotCanClass bot_can; diff --git a/src/imu_wrapper.hpp b/src/imu_wrapper.hpp index c03b3c6..f09cf56 100644 --- a/src/imu_wrapper.hpp +++ b/src/imu_wrapper.hpp @@ -4,7 +4,7 @@ #include #include "./SparkFun_BNO080_Arduino_Library.h" -#include "MkrRgb.hpp" +#include "portenta_rgb.hpp" #include "utils.hpp" constexpr float r2d(const float rad) { @@ -56,18 +56,19 @@ struct ImuWrapper { float yaw = 0.0f; float yaw_rate = 0.0f; - bool begin() { - Wire.begin(); + bool begin(TwoWire& i2cPort) { + i2cPort.begin(); // Serial.println("Wire started"); // _imu.enableDebugging(Serial); - while (!_imu.begin(0x4A)) { - pixel.setColor(127, 0, 0); + pixel.setColor(127, 0, 0); + while (!_imu.begin(0x4A, i2cPort)) { + delayMicroseconds(100); } pixel.setColor(0, 0, 127); - Wire.setClock(400000); + i2cPort.setClock(400000); // Serial.println("Wire clock set"); // _imu.enableGyroIntegratedRotationVector(10); diff --git a/src/portenta_rgb.hpp b/src/portenta_rgb.hpp new file mode 100644 index 0000000..47cc2ff --- /dev/null +++ b/src/portenta_rgb.hpp @@ -0,0 +1,19 @@ +#pragma once + +struct RgbC33 { + void setup() { + pinMode(LEDR, OUTPUT); + pinMode(LEDG, OUTPUT); + pinMode(LEDB, OUTPUT); + + setColor(0, 50, 0); + } + + void setColor(uint8_t R, uint8_t G, uint8_t B) { + analogWrite(LEDR, 255 - R); + analogWrite(LEDG, 255 - G); + analogWrite(LEDB, 255 - B); + } +}; + +extern RgbC33 pixel; From 061fc0513db76330040b87986d3019960c445668 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 20 Jun 2024 01:17:24 -0700 Subject: [PATCH 02/12] Using fixed CAN lib with Tx Buffers --- src/balancer.hpp | 8 ++++---- src/bot_can.hpp | 11 ++++------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/balancer.hpp b/src/balancer.hpp index b078e07..ece9af0 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -112,11 +112,11 @@ struct BotController { bot_can.left_motor.set_input_torque_msg.Input_Torque = 0.0f; } - Serial.print("Pitch: "); - Serial.print(imu.pitch); + // Serial.print("Pitch: "); + // Serial.print(imu.pitch); - Serial.print("\tVel: "); - Serial.println(vel_actual); + // Serial.print("\tVel: "); + // Serial.println(vel_actual); } State run_state_machine(State state) { diff --git a/src/bot_can.hpp b/src/bot_can.hpp index a985754..f756708 100644 --- a/src/bot_can.hpp +++ b/src/bot_can.hpp @@ -13,15 +13,13 @@ void sendCanMsg(const can_Message_t &msg) { if (ret != 1) { Serial.print("Tx Failed: "); - Serial.print(ret, HEX); + Serial.print(ret); Serial.print(" - "); Serial.println(c33msg); - } else { - Serial.println("Tx Success"); } // TODO: Figure out why this is needed - no FIFO in R7FA6M5_CAN ??? - delayMicroseconds(500); + // delayMicroseconds(500); } struct BotCanClass { @@ -35,9 +33,6 @@ struct BotCanClass { left_motor.axis_id_ = 0; right_motor.axis_id_ = 1; - // Start CAN at 500kbps - CAN1.disableInternalLoopback(); - if (!CAN1.begin(CanBitRate::BR_500k)) { Serial.println("CAN Begin Failed!"); } else { @@ -76,6 +71,8 @@ struct BotCanClass { } void send() { + CAN1.clearError(); + // Send the periodic CAN messages sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); From fe5490b8d6ea138b7141fa2a58111754fdff53dd Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 17 Aug 2024 17:34:52 -0700 Subject: [PATCH 03/12] Decode all CAN messages --- src/balancer.hpp | 17 +++++++++++++ src/can_simple_messages.hpp | 48 ++++++++++++++++++------------------- 2 files changed, 41 insertions(+), 24 deletions(-) diff --git a/src/balancer.hpp b/src/balancer.hpp index ece9af0..83d6638 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -149,18 +149,35 @@ struct BotController { bot_can.setAxisStates(AXIS_STATE_IDLE); next_state = State::Idle; + + if (pitch_over) { + Serial.println("Pitch Over"); + } + + if (left_error) { + Serial.print("Left Error: "); + Serial.println(bot_can.left_motor.heartbeat_msg.Axis_Error, HEX); + } + + if (right_error) { + Serial.print("Right Error: "); + Serial.println(bot_can.right_motor.heartbeat_msg.Axis_Error, HEX); + } } if (imu_timeout) { bot_can.setAxisStates(AXIS_STATE_IDLE); next_state = State::Error; + Serial.println("IMU Timeout"); } } break; case State::Error: + Serial.println("Error State!"); default: { bot_can.setAxisStates(AXIS_STATE_IDLE); + Serial.println("Invalid State!"); } break; } diff --git a/src/can_simple_messages.hpp b/src/can_simple_messages.hpp index da42aec..f8c603f 100644 --- a/src/can_simple_messages.hpp +++ b/src/can_simple_messages.hpp @@ -684,32 +684,32 @@ struct ODriveArduinoCAN { void decode(const can_Message_t& msg) { switch (get_cmd_id(msg.id)) { - // case kGetVersionMsg: get_version_msg.decode(msg); break; + case kGetVersionMsg: get_version_msg.decode(msg); break; case kHeartbeatMsg: heartbeat_msg.decode(msg); break; - // case kEstopMsg: estop_msg.decode(msg); break; - // case kGetErrorMsg: get_error_msg.decode(msg); break; - // case kSetAxisNodeIDMsg: set_axis_node_id_msg.decode(msg); break; - // case kSetAxisStateMsg: set_axis_state_msg.decode(msg); break; + case kEstopMsg: estop_msg.decode(msg); break; + case kGetErrorMsg: get_error_msg.decode(msg); break; + case kSetAxisNodeIDMsg: set_axis_node_id_msg.decode(msg); break; + case kSetAxisStateMsg: set_axis_state_msg.decode(msg); break; case kGetEncoderEstimatesMsg: get_encoder_estimates_msg.decode(msg); break; - // case kSetControllerModeMsg: set_controller_mode_msg.decode(msg); break; - // case kSetInputPosMsg: set_input_pos_msg.decode(msg); break; - // case kSetInputVelMsg: set_input_vel_msg.decode(msg); break; - // case kSetInputTorqueMsg: set_input_torque_msg.decode(msg); break; - // case kSetLimitsMsg: set_limits_msg.decode(msg); break; - // case kSetTrajVelLimitMsg: set_traj_vel_limit_msg.decode(msg); break; - // case kSetTrajAccelLimitsMsg: set_traj_accel_limits_msg.decode(msg); break; - // case kSetTrajInertiaMsg: set_traj_inertia_msg.decode(msg); break; - // case kGetIqMsg: get_iq_msg.decode(msg); break; - // case kGetTemperatureMsg: get_temperature_msg.decode(msg); break; - // case kRebootMsg: reboot_msg.decode(msg); break; - // case kGetBusVoltageCurrentMsg: get_bus_voltage_current_msg.decode(msg); break; - // case kClearErrorsMsg: clear_errors_msg.decode(msg); break; - // case kSetAbsolutePositionMsg: set_absolute_position_msg.decode(msg); break; - // case kSetPosGainMsg: set_pos_gain_msg.decode(msg); break; - // case kSetVelGainsMsg: set_vel_gains_msg.decode(msg); break; - // case kGetTorquesMsg: get_torques_msg.decode(msg); break; - // case kGetControllerErrorMsg: get_controller_error_msg.decode(msg); break; - // case kEnterDFUModeMsg: enter_dfu_mode_msg.decode(msg); break; + case kSetControllerModeMsg: set_controller_mode_msg.decode(msg); break; + case kSetInputPosMsg: set_input_pos_msg.decode(msg); break; + case kSetInputVelMsg: set_input_vel_msg.decode(msg); break; + case kSetInputTorqueMsg: set_input_torque_msg.decode(msg); break; + case kSetLimitsMsg: set_limits_msg.decode(msg); break; + case kSetTrajVelLimitMsg: set_traj_vel_limit_msg.decode(msg); break; + case kSetTrajAccelLimitsMsg: set_traj_accel_limits_msg.decode(msg); break; + case kSetTrajInertiaMsg: set_traj_inertia_msg.decode(msg); break; + case kGetIqMsg: get_iq_msg.decode(msg); break; + case kGetTemperatureMsg: get_temperature_msg.decode(msg); break; + case kRebootMsg: reboot_msg.decode(msg); break; + case kGetBusVoltageCurrentMsg: get_bus_voltage_current_msg.decode(msg); break; + case kClearErrorsMsg: clear_errors_msg.decode(msg); break; + case kSetAbsolutePositionMsg: set_absolute_position_msg.decode(msg); break; + case kSetPosGainMsg: set_pos_gain_msg.decode(msg); break; + case kSetVelGainsMsg: set_vel_gains_msg.decode(msg); break; + case kGetTorquesMsg: get_torques_msg.decode(msg); break; + case kGetControllerErrorMsg: get_controller_error_msg.decode(msg); break; + case kEnterDFUModeMsg: enter_dfu_mode_msg.decode(msg); break; default: break; } }; From 027bef851dafdae34c5812cce4f7cbe08f84a820 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Fri, 15 Nov 2024 22:44:16 -0800 Subject: [PATCH 04/12] Changes --- .vscode/settings.json | 6 +++++- BalanceBot.ino | 15 ++++++++++++++- src/balancer.hpp | 32 +++++++++++++++++--------------- src/bot_can.hpp | 3 --- 4 files changed, 36 insertions(+), 20 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 903e16f..c6fd32b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -11,6 +11,10 @@ "vector": "cpp", "string_view": "cpp", "memory": "cpp", - "initializer_list": "cpp" + "initializer_list": "cpp", + "atomic": "cpp", + "streambuf": "cpp", + "typeinfo": "cpp", + "utility": "cpp" }, } \ No newline at end of file diff --git a/BalanceBot.ino b/BalanceBot.ino index 58a5987..635f8b5 100644 --- a/BalanceBot.ino +++ b/BalanceBot.ino @@ -27,8 +27,11 @@ RgbC33 pixel; // Task handles static TaskHandle_t taskHandle_1kHz; static TaskHandle_t taskHandle_100Hz; +static TaskHandle_t taskHandle_1Hz; void setup() { + pinMode(PIN_D7, PinMode::OUTPUT); + configControllers(); // Initialize Serial @@ -48,6 +51,7 @@ void setup() { // Create RTOS tasks xTaskCreate(periodic_1kHz, "IMU Task", 256, nullptr, tskIDLE_PRIORITY + 3, &taskHandle_1kHz); xTaskCreate(periodic_100Hz, "CAN Task", 256, nullptr, tskIDLE_PRIORITY + 2, &taskHandle_100Hz); + xTaskCreate(periodic_1Hz, "Beep task", 256, nullptr, tskIDLE_PRIORITY + 1, &taskHandle_1Hz); // Start RTOS tasks Serial.println("Starting Scheduler"); @@ -62,6 +66,14 @@ void setup() { } } +static void periodic_1Hz(void *pvParameters) { + TickType_t lastWakeTime = xTaskGetTickCount(); + for (;;) { + vTaskDelayUntil(&lastWakeTime, 1000UL); + Serial.println("."); + } +} + static void periodic_100Hz(void *pvParameters) { TickType_t lastWakeTime = xTaskGetTickCount(); @@ -75,7 +87,8 @@ static void periodic_100Hz(void *pvParameters) { } static void periodic_1kHz(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); + TickType_t lastWakeTime = xTaskGetTickCount(); + static bool flag = false; // Run this code periodically at 1kHz for (;;) { diff --git a/src/balancer.hpp b/src/balancer.hpp index 83d6638..9c09fbe 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -4,6 +4,7 @@ #include "imu_wrapper.hpp" #include "pid.hpp" #include "utils.hpp" +#include struct BalanceController { struct Settings_t { @@ -122,6 +123,20 @@ struct BotController { State run_state_machine(State state) { State next_state = state; + const bool left_error = bot_can.left_motor.heartbeat_msg.Axis_Error != 0; + const bool right_error = bot_can.right_motor.heartbeat_msg.Axis_Error != 0; + + digitalWrite(PIN_D7, left_error || right_error); + if (left_error) { + Serial.print("Left Error: "); + Serial.println(bot_can.left_motor.heartbeat_msg.Axis_Error, HEX); + } + + if (right_error) { + Serial.print("Right Error: "); + Serial.println(bot_can.right_motor.heartbeat_msg.Axis_Error, HEX); + } + switch (state) { case State::Idle: { // If pitch is within 5 degrees for 2 seconds, enable motors @@ -138,11 +153,8 @@ struct BotController { case State::Active: { // Check for errors - bool pitch_over = fabsf(imu.pitch) > 30.0f; - bool imu_timeout = imu.getIsTimedOut(); - - bool left_error = bot_can.left_motor.heartbeat_msg.Axis_Error != 0; - bool right_error = bot_can.right_motor.heartbeat_msg.Axis_Error != 0; + const bool pitch_over = fabsf(imu.pitch) > 30.0f; + const bool imu_timeout = imu.getIsTimedOut(); if (pitch_over || left_error || right_error) { vertical_timer.reset(); @@ -153,16 +165,6 @@ struct BotController { if (pitch_over) { Serial.println("Pitch Over"); } - - if (left_error) { - Serial.print("Left Error: "); - Serial.println(bot_can.left_motor.heartbeat_msg.Axis_Error, HEX); - } - - if (right_error) { - Serial.print("Right Error: "); - Serial.println(bot_can.right_motor.heartbeat_msg.Axis_Error, HEX); - } } if (imu_timeout) { diff --git a/src/bot_can.hpp b/src/bot_can.hpp index f756708..da6ab44 100644 --- a/src/bot_can.hpp +++ b/src/bot_can.hpp @@ -17,9 +17,6 @@ void sendCanMsg(const can_Message_t &msg) { Serial.print(" - "); Serial.println(c33msg); } - - // TODO: Figure out why this is needed - no FIFO in R7FA6M5_CAN ??? - // delayMicroseconds(500); } struct BotCanClass { From a29c03abf347c98954233698fab7df66d655e8ab Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 19:48:18 -0800 Subject: [PATCH 05/12] Use platformio --- .gitignore | 41 +------ .vscode/settings.json | 20 ---- {src => include}/MkrRgb.hpp | 0 {src => include}/ODriveEnums.h | 0 include/README | 37 ++++++ .../SparkFun_BNO080_Arduino_Library.h | 0 {src => include}/balancer.hpp | 0 {src => include}/bot_can.hpp | 0 {src => include}/can_helpers.hpp | 0 {src => include}/can_simple_messages.hpp | 0 {src => include}/config.hpp | 0 {src => include}/imu_wrapper.hpp | 0 {src => include}/pid.hpp | 0 {src => include}/portenta_rgb.hpp | 0 {src => include}/utils.hpp | 0 lib/README | 46 +++++++ platformio.ini | 18 +++ BalanceBot.ino => src/main.cpp | 113 ++++++++++-------- test/README | 11 ++ 19 files changed, 176 insertions(+), 110 deletions(-) delete mode 100644 .vscode/settings.json rename {src => include}/MkrRgb.hpp (100%) rename {src => include}/ODriveEnums.h (100%) create mode 100644 include/README rename {src => include}/SparkFun_BNO080_Arduino_Library.h (100%) rename {src => include}/balancer.hpp (100%) rename {src => include}/bot_can.hpp (100%) rename {src => include}/can_helpers.hpp (100%) rename {src => include}/can_simple_messages.hpp (100%) rename {src => include}/config.hpp (100%) rename {src => include}/imu_wrapper.hpp (100%) rename {src => include}/pid.hpp (100%) rename {src => include}/portenta_rgb.hpp (100%) rename {src => include}/utils.hpp (100%) create mode 100644 lib/README create mode 100644 platformio.ini rename BalanceBot.ino => src/main.cpp (69%) create mode 100644 test/README diff --git a/.gitignore b/.gitignore index fc67c02..45fcef7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,38 +1,3 @@ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -build/ -.vscode/c_cpp_properties.json - -WiFi_secrets.hpp -arduino.json \ No newline at end of file +.pio +.vscode/ +.cache/ \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index c6fd32b..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "files.associations": { - "*.config": "makefile", - "cmath": "cpp", - "algorithm": "cpp", - "array": "cpp", - "*.tcc": "cpp", - "deque": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "string_view": "cpp", - "memory": "cpp", - "initializer_list": "cpp", - "atomic": "cpp", - "streambuf": "cpp", - "typeinfo": "cpp", - "utility": "cpp" - }, -} \ No newline at end of file diff --git a/src/MkrRgb.hpp b/include/MkrRgb.hpp similarity index 100% rename from src/MkrRgb.hpp rename to include/MkrRgb.hpp diff --git a/src/ODriveEnums.h b/include/ODriveEnums.h similarity index 100% rename from src/ODriveEnums.h rename to include/ODriveEnums.h diff --git a/include/README b/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/src/SparkFun_BNO080_Arduino_Library.h b/include/SparkFun_BNO080_Arduino_Library.h similarity index 100% rename from src/SparkFun_BNO080_Arduino_Library.h rename to include/SparkFun_BNO080_Arduino_Library.h diff --git a/src/balancer.hpp b/include/balancer.hpp similarity index 100% rename from src/balancer.hpp rename to include/balancer.hpp diff --git a/src/bot_can.hpp b/include/bot_can.hpp similarity index 100% rename from src/bot_can.hpp rename to include/bot_can.hpp diff --git a/src/can_helpers.hpp b/include/can_helpers.hpp similarity index 100% rename from src/can_helpers.hpp rename to include/can_helpers.hpp diff --git a/src/can_simple_messages.hpp b/include/can_simple_messages.hpp similarity index 100% rename from src/can_simple_messages.hpp rename to include/can_simple_messages.hpp diff --git a/src/config.hpp b/include/config.hpp similarity index 100% rename from src/config.hpp rename to include/config.hpp diff --git a/src/imu_wrapper.hpp b/include/imu_wrapper.hpp similarity index 100% rename from src/imu_wrapper.hpp rename to include/imu_wrapper.hpp diff --git a/src/pid.hpp b/include/pid.hpp similarity index 100% rename from src/pid.hpp rename to include/pid.hpp diff --git a/src/portenta_rgb.hpp b/include/portenta_rgb.hpp similarity index 100% rename from src/portenta_rgb.hpp rename to include/portenta_rgb.hpp diff --git a/src/utils.hpp b/include/utils.hpp similarity index 100% rename from src/utils.hpp rename to include/utils.hpp diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..9379397 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into the executable file. + +The source code of each library should be placed in a separate directory +("lib/your_library_name/[Code]"). + +For example, see the structure of the following example libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Example contents of `src/main.c` using Foo and Bar: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +The PlatformIO Library Dependency Finder will find automatically dependent +libraries by scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..2ca53c1 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,18 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:portenta_c33] +platform = renesas-ra +platform_packages = + platformio/toolchain-gccarmnoneeabi@^1.120201.0 +board = portenta_c33 +framework = arduino +build_flags = + -ffast-math \ No newline at end of file diff --git a/BalanceBot.ino b/src/main.cpp similarity index 69% rename from BalanceBot.ino rename to src/main.cpp index 635f8b5..1ce8efd 100644 --- a/BalanceBot.ino +++ b/src/main.cpp @@ -1,35 +1,73 @@ - +#include "Arduino.h" #include #include +#include -#include - -#include "src/ODriveEnums.h" -#include "src/balancer.hpp" -#include "src/bot_can.hpp" -#include "src/can_helpers.hpp" -#include "src/can_simple_messages.hpp" -#include "src/config.hpp" -#include "src/imu_wrapper.hpp" -#include "src/portenta_rgb.hpp" -#include "src/utils.hpp" +#include +#include +#include +#include +#include +#include +#include +#include #ifdef ARDUINO_PORTENTA_C33 // TODO: Make it easy to configure this #endif // Global object initialization -BotCanClass bot_can; +BotCanClass bot_can; BotController controller; -ImuWrapper imu; -RgbC33 pixel; +ImuWrapper imu; +RgbC33 pixel; // Task handles static TaskHandle_t taskHandle_1kHz; static TaskHandle_t taskHandle_100Hz; static TaskHandle_t taskHandle_1Hz; -void setup() { +static void periodic_1Hz(void *pvParameters) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); + for (;;) + { + vTaskDelayUntil(&lastWakeTime, 1000UL); + Serial.println("."); + } +} + +static void periodic_100Hz(void *pvParameters) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); + + // Run this code periodically at 100Hz + for (;;) + { + vTaskDelayUntil(&lastWakeTime, 10UL); + + controller.step(); + bot_can.send(); + } +} + +static void periodic_1kHz(void *pvParameters) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); + static bool flag = false; + + // Run this code periodically at 1kHz + for (;;) + { + vTaskDelayUntil(&lastWakeTime, 1UL); + + imu.read(); + bot_can.read(); + } +} + +void setup() +{ pinMode(PIN_D7, PinMode::OUTPUT); configControllers(); @@ -38,7 +76,8 @@ void setup() { Serial.begin(115200); const uint32_t start = millis(); - while ((millis() - start < 1000) && !Serial) { + while ((millis() - start < 1000) && !Serial) + { delay(1); } @@ -59,47 +98,17 @@ void setup() { vTaskStartScheduler(); - for (;;) { + for (;;) + { Serial.println("Scheduler failed!"); Serial.flush(); delay(1000); } } -static void periodic_1Hz(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - for (;;) { - vTaskDelayUntil(&lastWakeTime, 1000UL); - Serial.println("."); - } -} - -static void periodic_100Hz(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - - // Run this code periodically at 100Hz - for (;;) { - vTaskDelayUntil(&lastWakeTime, 10UL); - - controller.step(); - bot_can.send(); - } -} - -static void periodic_1kHz(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - static bool flag = false; - - // Run this code periodically at 1kHz - for (;;) { - vTaskDelayUntil(&lastWakeTime, 1UL); - - imu.read(); - bot_can.read(); - } -} - // RTOS Idle Loop -void loop() { +void loop() +{ + delay(1000); } diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html From 96a2ac1e377a28c8cc0d42092cdfb2ae16a1a3a9 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 20:01:41 -0800 Subject: [PATCH 06/12] Ignore compilation database --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 45fcef7..baea8f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .pio .vscode/ -.cache/ \ No newline at end of file +.cache/ +compile_commands.json \ No newline at end of file From 62318d861811481e78cbced8a6d02aa5cb98b0dc Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 23:24:39 -0800 Subject: [PATCH 07/12] Remove blink --- include/balancer.hpp | 1 - include/utils.hpp | 13 ------------- platformio.ini | 1 + 3 files changed, 1 insertion(+), 14 deletions(-) diff --git a/include/balancer.hpp b/include/balancer.hpp index 9c09fbe..0c3838d 100644 --- a/include/balancer.hpp +++ b/include/balancer.hpp @@ -76,7 +76,6 @@ struct BotController { void step() { // Blink orange LED at 1 sec - bot::blink(1000); // Run State Machine state = run_state_machine(state); diff --git a/include/utils.hpp b/include/utils.hpp index d854ae5..707d48d 100644 --- a/include/utils.hpp +++ b/include/utils.hpp @@ -11,19 +11,6 @@ constexpr const T& clamp(const T& x, const T& lo, const T& hi) { return std::min(std::max(x, lo), hi); } -void blink(const uint32_t blink_period_ms) { - static uint32_t last_blink = millis(); - static bool led_state = false; - - const uint32_t now = millis(); - if ((now - last_blink) >= (blink_period_ms / 2UL)) { - last_blink = now; - led_state = !led_state; - - digitalWrite(LED_BUILTIN, led_state ? HIGH : LOW); - } -} - struct LPF { LPF(float Ts, float Tau) : _alpha(Ts / (Ts + Tau)) {} diff --git a/platformio.ini b/platformio.ini index 2ca53c1..21a5045 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,5 +14,6 @@ platform_packages = platformio/toolchain-gccarmnoneeabi@^1.120201.0 board = portenta_c33 framework = arduino +monitor_speed = 115200 build_flags = -ffast-math \ No newline at end of file From 6f99f2369c174446fed27eb20e866440043ddf6d Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 23:24:47 -0800 Subject: [PATCH 08/12] Improve CAN helpers --- include/can_helpers.hpp | 76 +++++++++++++++++++++++++++-------------- 1 file changed, 50 insertions(+), 26 deletions(-) diff --git a/include/can_helpers.hpp b/include/can_helpers.hpp index 2771323..0f392e7 100644 --- a/include/can_helpers.hpp +++ b/include/can_helpers.hpp @@ -3,26 +3,58 @@ #include #include -struct can_Message_t { - uint32_t id = 0x000; - uint8_t len = 0; - bool is_extended_id = false; - bool rtr = false; - uint8_t data[8] = {0}; +struct can_Message_t : public Printable { + can_Message_t() = default; + can_Message_t(const uint32_t id, const uint8_t len, const uint8_t (&data)[8], const bool is_extended_id = false, const bool rtr = false) + : id(id), len(len), is_extended_id(is_extended_id), rtr(rtr) { + std::memcpy(this->data, data, 8); + } + + uint32_t id = 0x000; + uint8_t len = 0; + bool is_extended_id = false; + bool rtr = false; + uint8_t data[8] = {0}; + + size_t printTo(Print& p) const override { + size_t n = 0; + + n += p.print("ID: 0x"); + n += p.print(id < 0x100 ? "0" : ""); + n += p.print(id < 0x10 ? "0" : ""); + n += p.print(id, HEX); + n += p.print(is_extended_id ? "x" : ""); + n += p.print(" Len: "); + n += p.print(len); + n += p.print(" Data: "); + + for (int i = 0; i < len; i++) { + if (data[i] < 0x10) { + n += p.print('0'); // Add leading zero for single digit hex values + } + n += p.print(data[i], HEX); + n += p.print(' '); + } + + if (rtr) { + n += p.print("(RTR)"); + } + + return n; + } }; template T can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t length, const bool isIntel) { union { - uint64_t tempVal; - uint8_t tempBuf[N]; // This is used because memcpy into tempVal generates less optimal code T retVal; }; - const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; - const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; + const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; + const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; - std::memcpy(tempBuf, buf, N); + uint64_t tempVal = 0U; + std::memcpy(&tempVal, buf, N); if (isIntel) { tempVal = (tempVal >> shift) & mask; } else { @@ -35,22 +67,14 @@ T can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t len template void can_setSignal(uint8_t (&buf)[N], const T& val, const size_t startBit, const size_t length, const bool isIntel) { - const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; - const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; - - union { - uint64_t valAsBits; - T tempVal; - }; + const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; + const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; - tempVal = val; - - union { - uint64_t data; - uint8_t tempBuf[N]; - }; + uint64_t valAsBits = 0; + std::memcpy(&valAsBits, &val, sizeof(T)); - std::memcpy(tempBuf, buf, N); + uint64_t data = 0; + std::memcpy(&data, buf, N); if (isIntel) { data &= ~(mask << shift); data |= valAsBits << shift; @@ -61,7 +85,7 @@ void can_setSignal(uint8_t (&buf)[N], const T& val, const size_t startBit, const data = __builtin_bswap64(data); } - std::memcpy(buf, tempBuf, N); + std::memcpy(buf, &data, N); } template From e3dcaeb03237c3bd01527f063ac13d19b98a1e57 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 23:24:58 -0800 Subject: [PATCH 09/12] Improve c33 pixel --- include/portenta_rgb.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/portenta_rgb.hpp b/include/portenta_rgb.hpp index 47cc2ff..c5fd46b 100644 --- a/include/portenta_rgb.hpp +++ b/include/portenta_rgb.hpp @@ -5,8 +5,9 @@ struct RgbC33 { pinMode(LEDR, OUTPUT); pinMode(LEDG, OUTPUT); pinMode(LEDB, OUTPUT); - - setColor(0, 50, 0); + digitalWrite(LEDR, HIGH); + digitalWrite(LEDG, HIGH); + digitalWrite(LEDB, HIGH); } void setColor(uint8_t R, uint8_t G, uint8_t B) { From c4e909b79863a50ac953ad86cb77884fb8dd72f2 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 22 Feb 2025 23:25:04 -0800 Subject: [PATCH 10/12] Fix enum array --- include/ODriveEnums.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/include/ODriveEnums.h b/include/ODriveEnums.h index 5352243..d336cf6 100644 --- a/include/ODriveEnums.h +++ b/include/ODriveEnums.h @@ -1,6 +1,7 @@ -#ifndef ODriveEnums_h -#define ODriveEnums_h +#pragma once + +#include // ODrive.GpioMode enum ODriveGpioMode { @@ -205,5 +206,3 @@ enum ODriveCanError { CAN_ERROR_NONE = 0x00000000, CAN_ERROR_DUPLICATE_CAN_IDS = 0x00000001, }; - -#endif From 5024bd7982fec9415a576c9aab26fdb08c33df65 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 23 Feb 2025 00:00:16 -0800 Subject: [PATCH 11/12] Blink blue light in 1Hz task --- src/main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 1ce8efd..ad4b984 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -30,10 +30,11 @@ static TaskHandle_t taskHandle_1Hz; static void periodic_1Hz(void *pvParameters) { TickType_t lastWakeTime = xTaskGetTickCount(); + uint8_t count = 0; for (;;) { vTaskDelayUntil(&lastWakeTime, 1000UL); - Serial.println("."); + digitalWrite(LEDB, count++ & 0x1); } } From 9a087d154038abe93b08801647c756dea648706c Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 23 Feb 2025 01:01:19 -0800 Subject: [PATCH 12/12] It lives once more --- include/balancer.hpp | 17 +++++++++++++---- include/bot_can.hpp | 27 +++++++++++---------------- include/can_helpers.hpp | 8 +++----- include/can_simple_messages.hpp | 4 ++-- include/imu_wrapper.hpp | 2 -- src/main.cpp | 7 +++---- 6 files changed, 32 insertions(+), 33 deletions(-) diff --git a/include/balancer.hpp b/include/balancer.hpp index 0c3838d..ccce999 100644 --- a/include/balancer.hpp +++ b/include/balancer.hpp @@ -1,10 +1,11 @@ #pragma once +#include + #include "bot_can.hpp" #include "imu_wrapper.hpp" #include "pid.hpp" #include "utils.hpp" -#include struct BalanceController { struct Settings_t { @@ -70,6 +71,8 @@ struct BotController { Error }; + static constexpr std::array StateText = {"Idle", "Active", "Error"}; + void begin() { vertical_timer.reset(); } @@ -113,10 +116,13 @@ struct BotController { } // Serial.print("Pitch: "); - // Serial.print(imu.pitch); + // Serial.println(imu.pitch); - // Serial.print("\tVel: "); - // Serial.println(vel_actual); + // Serial.print("\tL: "); + // Serial.print(vel_left); + // Serial.print("\tR: "); + // Serial.print(vel_right); + // Serial.println(); } State run_state_machine(State state) { @@ -182,6 +188,9 @@ struct BotController { } break; } + if (state != next_state) + Serial.println(StateText[static_cast(next_state)]); + return next_state; } diff --git a/include/bot_can.hpp b/include/bot_can.hpp index da6ab44..7e839bb 100644 --- a/include/bot_can.hpp +++ b/include/bot_can.hpp @@ -6,20 +6,17 @@ #include "can_simple_messages.hpp" // Helper function for sending can messages -void sendCanMsg(const can_Message_t &msg) { - const CanMsg c33msg{CanStandardId(msg.id), msg.len, msg.data}; - auto ret = CAN1.write(c33msg); +struct BotCanClass { + void sendCanMsg(const can_Message_t &msg) { + const CanMsg c33msg{CanStandardId(msg.id), msg.len, msg.data}; - if (ret != 1) { - Serial.print("Tx Failed: "); - Serial.print(ret); - Serial.print(" - "); - Serial.println(c33msg); + if (m_rx_once) { + if (CAN1.write(c33msg) < 0) + Serial.println(msg); + } } -} -struct BotCanClass { void setup() { // Portenta C33 FD transceiver pins #ifdef ARDUINO_PORTENTA_C33 @@ -49,14 +46,11 @@ struct BotCanClass { void read() { while (CAN1.available()) { + m_rx_once = true; const CanMsg rxmsg = CAN1.read(); - can_Message_t odrv_msg = { - .id = rxmsg.id, - .len = rxmsg.data_length - }; - - std::memcpy(odrv_msg.data, rxmsg.data, rxmsg.data_length); + can_Message_t odrv_msg{rxmsg.id, rxmsg.data_length, rxmsg.data}; + // Serial.println(rxmsg); // pixel.setColor(0, led, led); switch (ODriveArduinoCAN::get_node_id(rxmsg.id)) { @@ -93,6 +87,7 @@ struct BotCanClass { ODriveArduinoCAN right_motor; // Node ID 1 bool m_axis_state_update = false; + bool m_rx_once = false; }; extern BotCanClass bot_can; diff --git a/include/can_helpers.hpp b/include/can_helpers.hpp index 0f392e7..75cacaf 100644 --- a/include/can_helpers.hpp +++ b/include/can_helpers.hpp @@ -46,15 +46,11 @@ struct can_Message_t : public Printable { template T can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t length, const bool isIntel) { - union { - T retVal; - }; - const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; const uint8_t shift = isIntel ? startBit : (64 - startBit) - length; uint64_t tempVal = 0U; - std::memcpy(&tempVal, buf, N); + std::memcpy(&tempVal, &buf[0], N); if (isIntel) { tempVal = (tempVal >> shift) & mask; } else { @@ -62,6 +58,8 @@ T can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t len tempVal = (tempVal >> shift) & mask; } + T retVal; + std::memcpy(&retVal, &tempVal, sizeof(T)); return retVal; } diff --git a/include/can_simple_messages.hpp b/include/can_simple_messages.hpp index f8c603f..23cbac4 100644 --- a/include/can_simple_messages.hpp +++ b/include/can_simple_messages.hpp @@ -187,8 +187,8 @@ struct Get_Encoder_Estimates_msg_t final { } void decode(const can_Message_t& msg) { - Pos_Estimate = can_getSignal(msg.data, 0, 32, true, 1.0f, 0.0f); - Vel_Estimate = can_getSignal(msg.data, 32, 32, true, 1.0f, 0.0f); + Pos_Estimate = can_getSignal(msg.data, 0, 32, true); + Vel_Estimate = can_getSignal(msg.data, 32, 32, true); } static constexpr uint8_t cmd_id = 0x009; diff --git a/include/imu_wrapper.hpp b/include/imu_wrapper.hpp index f09cf56..be297d6 100644 --- a/include/imu_wrapper.hpp +++ b/include/imu_wrapper.hpp @@ -62,11 +62,9 @@ struct ImuWrapper { // _imu.enableDebugging(Serial); - pixel.setColor(127, 0, 0); while (!_imu.begin(0x4A, i2cPort)) { delayMicroseconds(100); } - pixel.setColor(0, 0, 127); i2cPort.setClock(400000); // Serial.println("Wire clock set"); diff --git a/src/main.cpp b/src/main.cpp index ad4b984..6aba284 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -70,7 +70,6 @@ static void periodic_1kHz(void *pvParameters) void setup() { pinMode(PIN_D7, PinMode::OUTPUT); - configControllers(); // Initialize Serial @@ -89,9 +88,9 @@ void setup() controller.begin(); // Create RTOS tasks - xTaskCreate(periodic_1kHz, "IMU Task", 256, nullptr, tskIDLE_PRIORITY + 3, &taskHandle_1kHz); - xTaskCreate(periodic_100Hz, "CAN Task", 256, nullptr, tskIDLE_PRIORITY + 2, &taskHandle_100Hz); - xTaskCreate(periodic_1Hz, "Beep task", 256, nullptr, tskIDLE_PRIORITY + 1, &taskHandle_1Hz); + xTaskCreate(periodic_1kHz, "IMU Task", 1024, nullptr, tskIDLE_PRIORITY + 3, &taskHandle_1kHz); + xTaskCreate(periodic_100Hz, "CAN Task", 1024, nullptr, tskIDLE_PRIORITY + 2, &taskHandle_100Hz); + xTaskCreate(periodic_1Hz, "Beep task", 1024, nullptr, tskIDLE_PRIORITY + 1, &taskHandle_1Hz); // Start RTOS tasks Serial.println("Starting Scheduler");