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..baea8f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,37 +1,4 @@ -# 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 +.pio +.vscode/ +.cache/ +compile_commands.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/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 903e16f..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,16 +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" - }, -} \ No newline at end of file diff --git a/BalanceBot.ino b/BalanceBot.ino deleted file mode 100644 index 454b15d..0000000 --- a/BalanceBot.ino +++ /dev/null @@ -1,121 +0,0 @@ - -#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/utils.hpp" - -// Global object initialization -BotCanClass bot_can; -BotController controller; -ImuWrapper imu; -MKRrgb 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}; - -void setup() { - configControllers(); - - // Initialize Serial - Serial.begin(115200); - - const uint32_t start = millis(); - while ((millis() - start < 1000) && !Serial) { - delay(1); - } - - // Initialize MKR RGB LED - pixel.setup(); - - // 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); - - // Start RTOS tasks - Serial.println("Starting Scheduler"); - Serial.flush(); - - vTaskStartScheduler(); - - for (;;) { - Serial.println("Scheduler failed!"); - Serial.flush(); - delay(1000); - } -} - -// RTOS Idle Loop -void loop() { - delay(1000); -} - -static void controlTask(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) { - 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(); - } -} - -static void wifiTask(void *pvParameters) { - TickType_t lastWakeTime = xTaskGetTickCount(); - - Wifisetup(server); - - // Run this code periodically - for (;;) { - Wifiloop(server); - } -} 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 99% rename from src/ODriveEnums.h rename to include/ODriveEnums.h index 5352243..d336cf6 100644 --- a/src/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 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 99% rename from src/SparkFun_BNO080_Arduino_Library.h rename to include/SparkFun_BNO080_Arduino_Library.h index 37269f9..581adef 100644 --- a/src/SparkFun_BNO080_Arduino_Library.h +++ b/include/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/balancer.hpp b/include/balancer.hpp similarity index 79% rename from src/balancer.hpp rename to include/balancer.hpp index 874898f..ccce999 100644 --- a/src/balancer.hpp +++ b/include/balancer.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include "bot_can.hpp" #include "imu_wrapper.hpp" #include "pid.hpp" @@ -69,13 +71,14 @@ struct BotController { Error }; + static constexpr std::array StateText = {"Idle", "Active", "Error"}; + void begin() { vertical_timer.reset(); } void step() { // Blink orange LED at 1 sec - bot::blink(1000); // Run State Machine state = run_state_machine(state); @@ -91,8 +94,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 @@ -112,16 +115,33 @@ 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.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) { 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,32 +158,39 @@ 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(); bot_can.setAxisStates(AXIS_STATE_IDLE); next_state = State::Idle; + + if (pitch_over) { + Serial.println("Pitch Over"); + } } 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; } + 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 new file mode 100644 index 0000000..7e839bb --- /dev/null +++ b/include/bot_can.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include + +#include "ODriveEnums.h" +#include "can_simple_messages.hpp" + +// Helper function for sending can messages + +struct BotCanClass { + void sendCanMsg(const can_Message_t &msg) { + const CanMsg c33msg{CanStandardId(msg.id), msg.len, msg.data}; + + if (m_rx_once) { + if (CAN1.write(c33msg) < 0) + Serial.println(msg); + } + } + + 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; + + if (!CAN1.begin(CanBitRate::BR_500k)) { + Serial.println("CAN Begin Failed!"); + } else { + Serial.println("Connected to CAN at 500kbps"); + } + + sendCanMsg(left_motor.encode(ODriveArduinoCAN::kClearErrorsMsg)); + sendCanMsg(right_motor.encode(ODriveArduinoCAN::kClearErrorsMsg)); + } + + void setAxisStates(ODriveAxisState state) { + left_motor.set_axis_state_msg.Axis_Requested_State = state; + right_motor.set_axis_state_msg.Axis_Requested_State = state; + + m_axis_state_update = true; + } + + void read() { + while (CAN1.available()) { + m_rx_once = true; + const CanMsg rxmsg = CAN1.read(); + + 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)) { + case 0: left_motor.decode(odrv_msg); break; + case 1: right_motor.decode(odrv_msg); break; + default: break; + } + } + } + + void send() { + CAN1.clearError(); + + // 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; + bool m_rx_once = false; +}; + +extern BotCanClass bot_can; diff --git a/include/can_helpers.hpp b/include/can_helpers.hpp new file mode 100644 index 0000000..75cacaf --- /dev/null +++ b/include/can_helpers.hpp @@ -0,0 +1,99 @@ +#pragma once + +#include +#include + +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) { + 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[0], N); + if (isIntel) { + tempVal = (tempVal >> shift) & mask; + } else { + tempVal = __builtin_bswap64(tempVal); + tempVal = (tempVal >> shift) & mask; + } + + T retVal; + std::memcpy(&retVal, &tempVal, sizeof(T)); + return retVal; +} + +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; + + uint64_t valAsBits = 0; + std::memcpy(&valAsBits, &val, sizeof(T)); + + uint64_t data = 0; + std::memcpy(&data, buf, N); + if (isIntel) { + data &= ~(mask << shift); + data |= valAsBits << shift; + } else { + data = __builtin_bswap64(data); + data &= ~(mask << shift); + data |= valAsBits << shift; + data = __builtin_bswap64(data); + } + + std::memcpy(buf, &data, N); +} + +template +float can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t length, const bool isIntel, const float factor, const float offset) { + T retVal = can_getSignal(buf, startBit, length, isIntel); + return (retVal * factor) + offset; +} + +template +void can_setSignal(uint8_t (&buf)[N], const float& val, const size_t startBit, const size_t length, const bool isIntel, const float factor, const float offset) { + T scaledVal = static_cast((val - offset) / factor); + can_setSignal(buf, scaledVal, startBit, length, isIntel); +} diff --git a/src/can_simple_messages.hpp b/include/can_simple_messages.hpp similarity index 93% rename from src/can_simple_messages.hpp rename to include/can_simple_messages.hpp index da42aec..23cbac4 100644 --- a/src/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; @@ -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; } }; 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 93% rename from src/imu_wrapper.hpp rename to include/imu_wrapper.hpp index c03b3c6..be297d6 100644 --- a/src/imu_wrapper.hpp +++ b/include/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,17 @@ 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); + 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/pid.hpp b/include/pid.hpp similarity index 100% rename from src/pid.hpp rename to include/pid.hpp diff --git a/include/portenta_rgb.hpp b/include/portenta_rgb.hpp new file mode 100644 index 0000000..c5fd46b --- /dev/null +++ b/include/portenta_rgb.hpp @@ -0,0 +1,20 @@ +#pragma once + +struct RgbC33 { + void setup() { + pinMode(LEDR, OUTPUT); + pinMode(LEDG, OUTPUT); + pinMode(LEDB, OUTPUT); + digitalWrite(LEDR, HIGH); + digitalWrite(LEDG, HIGH); + digitalWrite(LEDB, HIGH); + } + + 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; diff --git a/src/utils.hpp b/include/utils.hpp similarity index 77% rename from src/utils.hpp rename to include/utils.hpp index d854ae5..707d48d 100644 --- a/src/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/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..21a5045 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,19 @@ +; 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 +monitor_speed = 115200 +build_flags = + -ffast-math \ No newline at end of file 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/bot_can.hpp b/src/bot_can.hpp deleted file mode 100644 index b2e8d89..0000000 --- a/src/bot_can.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -// Adafruit Feather M4 CAN -#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(); -} - -struct BotCanClass { - void setup() { - left_motor.axis_id_ = 0; - right_motor.axis_id_ = 1; - - // Start CAN at 500kbps - if (!CAN.begin(500000)) { - Serial.println("CAN Begin Failed!"); - } else { - Serial.println("Connected to CAN at 500kbps"); - } - - sendCanMsg(left_motor.encode(ODriveArduinoCAN::kClearErrorsMsg)); - sendCanMsg(right_motor.encode(ODriveArduinoCAN::kClearErrorsMsg)); - } - - void setAxisStates(ODriveAxisState state) { - 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)); - } - - void read() { - while (CAN.parsePacket()) { - can_Message_t rxmsg; - - rxmsg.id = CAN.packetId(); - CAN.readBytes(rxmsg.data, 8); - - // 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; - default: break; - } - } - } - - void send() { - // Send the periodic CAN messages - sendCanMsg(left_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); - sendCanMsg(right_motor.encode(ODriveArduinoCAN::kSetInputTorqueMsg)); - } - - // CAN communication objects for the ODrives - ODriveArduinoCAN left_motor; // Node ID 0 - ODriveArduinoCAN right_motor; // Node ID 1 -}; - -extern BotCanClass bot_can; diff --git a/src/can_helpers.hpp b/src/can_helpers.hpp deleted file mode 100644 index 2771323..0000000 --- a/src/can_helpers.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#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}; -}; - -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; - - std::memcpy(tempBuf, buf, N); - if (isIntel) { - tempVal = (tempVal >> shift) & mask; - } else { - tempVal = __builtin_bswap64(tempVal); - tempVal = (tempVal >> shift) & mask; - } - - return retVal; -} - -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; - }; - - tempVal = val; - - union { - uint64_t data; - uint8_t tempBuf[N]; - }; - - std::memcpy(tempBuf, buf, N); - if (isIntel) { - data &= ~(mask << shift); - data |= valAsBits << shift; - } else { - data = __builtin_bswap64(data); - data &= ~(mask << shift); - data |= valAsBits << shift; - data = __builtin_bswap64(data); - } - - std::memcpy(buf, tempBuf, N); -} - -template -float can_getSignal(const uint8_t (&buf)[N], const size_t startBit, const size_t length, const bool isIntel, const float factor, const float offset) { - T retVal = can_getSignal(buf, startBit, length, isIntel); - return (retVal * factor) + offset; -} - -template -void can_setSignal(uint8_t (&buf)[N], const float& val, const size_t startBit, const size_t length, const bool isIntel, const float factor, const float offset) { - T scaledVal = static_cast((val - offset) / factor); - can_setSignal(buf, scaledVal, startBit, length, isIntel); -} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..6aba284 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,114 @@ +#include "Arduino.h" +#include +#include +#include + +#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; +BotController controller; +ImuWrapper imu; +RgbC33 pixel; + +// Task handles +static TaskHandle_t taskHandle_1kHz; +static TaskHandle_t taskHandle_100Hz; +static TaskHandle_t taskHandle_1Hz; + +static void periodic_1Hz(void *pvParameters) +{ + TickType_t lastWakeTime = xTaskGetTickCount(); + uint8_t count = 0; + for (;;) + { + vTaskDelayUntil(&lastWakeTime, 1000UL); + digitalWrite(LEDB, count++ & 0x1); + } +} + +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(); + + // Initialize Serial + Serial.begin(115200); + + const uint32_t start = millis(); + while ((millis() - start < 1000) && !Serial) + { + delay(1); + } + + // Init objects + pixel.setup(); + imu.begin(Wire2); + bot_can.setup(); + controller.begin(); + + // Create RTOS tasks + 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"); + Serial.flush(); + + vTaskStartScheduler(); + + for (;;) + { + Serial.println("Scheduler failed!"); + Serial.flush(); + delay(1000); + } +} + +// RTOS Idle 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