diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py new file mode 100644 index 00000000..0a7be5f1 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py @@ -0,0 +1,270 @@ +"""Real-time Isaac Sim viewer for the physical bimanual test stand (Isaac Lab +counterpart to live_arm_mjviser.py, same directory). + +Drives one of BIMANUAL_ARM_CFG's two joint chains from live /interfacing/motorFeedback. +--arm-side picks which hardware_mapping.yaml motors to read (only "left" is +wired/calibrated today); --urdf-side picks which URDF chain to animate. Defaults to +--urdf-side right (unsuffixed joint1..joint6) -- confirmed against the real test stand +that this is the visually correct chain, NOT joint1L..joint6l as bimanual_arm_cfg.py's +docstring claims. Read-only: force-writes joint state via write_joint_state_to_sim() +each tick (no PD lag, exact mirror); never commands the real arm. + +Zero position: zero_offset + direction*motor_deg + display_offset per joint, matching +live_arm_mjviser.py and BIMANUAL_ARM_CFG's rest pose, so unwired joints (e.g. +wrist_pitch) sit at calibrated zero rather than a stale snapshot. + +For better compatibility with everyone's system, feedback comes via UDP from feedback_to_udp_bridge.py (system ROS +Python) instead of a direct subscription. + +Terminal 1 (system python, ROS sourced): + source /opt/ros/jazzy/setup.bash + source /home/rwahib/wato/humanoid/autonomy/install/setup.bash + /usr/bin/python3 autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py + +Terminal 2 (env_isaaclab): + conda activate env_isaaclab + cd /home/rwahib/wato/humanoid + python autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py +""" + + +import argparse +import os +import socket +import struct +import sys + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Live Isaac Sim mirror of the real bimanual arm.") +parser.add_argument("--arm-side", default="left", choices=["left", "right"], + help="hardware side: which hardware_mapping.yaml section / real motors to read " + "(only 'left' is wired/calibrated today)") +parser.add_argument("--urdf-side", default="right", choices=["left", "right"], + help="which URDF chain to drive with that feedback: 'right' -> unsuffixed " + "joint1..joint6 (default, matches live_arm_mjviser.py's validated " + "ARM_BRINGUP.md invocation), 'left' -> suffixed joint1L..joint6l " + "(the chain task_space_real.py drives for real hardware output).") +parser.add_argument("--flip", nargs="*", default=[], metavar="LABEL", + help="hardware_mapping labels whose sign to invert (e.g. shoulder_roll) -- " + "same as live_arm_mjviser.py's --flip.") +parser.add_argument("--offset", nargs="*", default=[], metavar="LABEL=DEG", + help="viewer-only constant added to a joint's displayed angle, in degrees " + "(e.g. shoulder_yaw=90) -- same as live_arm_mjviser.py's --offset. " + "Never touches real commands.") +parser.add_argument("--host", type=str, default="127.0.0.1", help="feedback_to_udp_bridge.py host") +parser.add_argument("--port", type=int, default=5006, help="feedback_to_udp_bridge.py port") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# Import bimanual_arm_cfg from keyboard teleoperation (same robot model/zero convention +# as task_space_real.py and live_arm_mjviser.py). +_KEYBOARD_TELEOP_DIR = os.path.abspath( + os.path.join(os.path.dirname(__file__), "../../Teleop/keyboard_based_teleoperation") +) +sys.path.insert(0, _KEYBOARD_TELEOP_DIR) + +import yaml # noqa: E402 + +from bimanual_arm_cfg import ( # noqa: E402 + BIMANUAL_ARM_CFG, + _HARDWARE_MAPPING_PATH, + apply_joint_limits, + resolve_joint_name, +) +import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.assets import AssetBaseCfg # noqa: E402 +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.utils import configclass # noqa: E402 +import math # noqa: E402 + + +# hardware_mapping.yaml label -> BIMANUAL_ARM_CFG URDF joint, per side. Same dict as +# live_arm_mjviser.py's LABEL_TO_URDF_JOINT (duplicated rather than imported from that +# script, since it pulls in rclpy/mujoco/viser at import time). +LABEL_TO_URDF_JOINT = { + "left": { + "shoulder_pitch": "joint1L", + "shoulder_roll": "joint2l", + "shoulder_yaw": "joint3l", + "elbow_pitch": "joint4l", + "elbow_roll": "joint5l", + "wrist_pitch": "joint6l", + }, + "right": { + "shoulder_pitch": "joint1", + "shoulder_roll": "joint2", + "shoulder_yaw": "joint3", + "elbow_pitch": "joint4", + "elbow_roll": "joint5", + "wrist_pitch": "joint6", + }, +} + + +def load_can_id_map( + mapping_path: str, + hw_side: str, + urdf_side: str, + flip_labels: set = frozenset(), + offset_labels: dict = None, +) -> dict: + """hardware_mapping.yaml -> {can_id: {label, urdf_joint, direction, zero_offset, display_offset, + lower_limit, upper_limit}}. + + Same fields, --flip/--offset semantics, and hw_side/urdf_side split as + live_arm_mjviser.py's load_can_id_map. + """ + with open(mapping_path) as f: + config = yaml.safe_load(f)[hw_side] + + label_to_joint = LABEL_TO_URDF_JOINT[urdf_side] + can_id_map = {} + for group, joints in config.items(): + for name, cfg in joints.items(): + label = f"{group}_{name}" + urdf_joint = label_to_joint.get(label) + if urdf_joint is None: + continue + # Must negate zero_offset together with direction, not direction alone: zero_offset was + # derived as -home_pos/direction so zero_offset + direction*home_pos == 0 at the real + # zero pose. Flipping direction alone breaks that and shifts the zero pose by + # 2*zero_offset. Negating both preserves joint_deg(home_pos)==0 while correctly + # reversing motion sense (verified: joint_deg_new(raw) == -joint_deg_old(raw)). + + flip = label in flip_labels + direction = int(cfg["direction"]) * (-1 if flip else 1) + zero_offset = float(cfg["zero_offset"]) * (-1 if flip else 1) + can_id_map[int(cfg["can_id"])] = { + "label": label, + "urdf_joint": urdf_joint, + "direction": direction, + "zero_offset": zero_offset, + "display_offset": float((offset_labels or {}).get(label, 0.0)), + "lower_limit": float(cfg["lower_limit"]), + "upper_limit": float(cfg["upper_limit"]), + } + return can_id_map + + +@configclass +class BareSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + ) + robot = BIMANUAL_ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene, + can_id_map: dict, sock: socket.socket) -> None: + robot = scene["robot"] + sim_dt = sim.get_physics_dt() + + scene.update(sim_dt) + apply_joint_limits(robot) + + name_to_id = {name: i for i, name in enumerate(robot.data.joint_names)} + urdf_joint_ids = { + cfg["urdf_joint"]: name_to_id[resolve_joint_name(robot, cfg["urdf_joint"])] + for cfg in can_id_map.values() + } + + print(f"Tracking {len(can_id_map)} motors from {_HARDWARE_MAPPING_PATH} " + f"(hardware={args_cli.arm_side} arm -> driving URDF {args_cli.urdf_side} arm):") + for can_id, cfg in sorted(can_id_map.items()): + flip = " (flipped)" if cfg["label"] in set(args_cli.flip) else "" + off = f" (offset {cfg['display_offset']:+g}°)" if cfg["display_offset"] else "" + print(f" 0x{can_id:02X} -> {cfg['label']:<14} -> {cfg['urdf_joint']}{flip}{off}") + + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + print("[INFO] Live Isaac Sim mirror running -- move the real arm to see it track here.") + + # Diagnostic only (no filtering/rejection -- this stays a raw, unfiltered mirror by + # design, same as live_arm_mjviser.py). Logs a jump's RAW inputs so a future glitch + # can be traced to its actual source: if the raw `position` printed here is already + # anomalous, it's upstream (CAN/encoder); if raw looks sane but joint_deg doesn't, + # it's a bug in this script's own transform. + _JUMP_WARN_DEG = 15.0 + _last_deg = {} + + while simulation_app.is_running(): + while True: + try: + data, _ = sock.recvfrom(1024) + except BlockingIOError: + break + if len(data) != struct.calcsize("=id"): + continue + motor_id, position = struct.unpack("=id", data) + cfg = can_id_map.get(motor_id) + if cfg is None: + continue + joint_deg = cfg["zero_offset"] + cfg["direction"] * position + cfg["display_offset"] + prev = _last_deg.get(motor_id) + if prev is not None and abs(joint_deg - prev) > _JUMP_WARN_DEG: + print(f"[WARN] {cfg['label']} (motor {motor_id}) jumped {prev:.2f}deg -> " + f"{joint_deg:.2f}deg (raw position={position:.3f}, packet_bytes={len(data)})") + _last_deg[motor_id] = joint_deg + joint_id = urdf_joint_ids[cfg["urdf_joint"]] + joint_position[0, joint_id] = math.radians(joint_deg) + + # Direct state overwrite (not set_joint_position_target): matches + # live_arm_mjviser.py's `data.qpos[...] = ...` -- an exact live mirror with no + # PD lag, since this is read-only visualization, not a commanded target. + robot.write_joint_state_to_sim(joint_position, joint_vel) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + +def main() -> None: + offset_labels = {} + for item in args_cli.offset: + label, _, val = item.partition("=") + offset_labels[label.strip()] = float(val) + + can_id_map = load_can_id_map( + _HARDWARE_MAPPING_PATH, args_cli.arm_side, args_cli.urdf_side, + set(args_cli.flip), offset_labels + ) + if not can_id_map: + raise RuntimeError( + f"No joints resolved for arm_side={args_cli.arm_side!r} in {_HARDWARE_MAPPING_PATH}" + ) + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind((args_cli.host, args_cli.port)) + sock.setblocking(False) + print(f"[UDP] Listening for motor feedback on {args_cli.host}:{args_cli.port} " + f"(run feedback_to_udp_bridge.py to feed this)") + + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + sim.set_camera_view([2.5, 2.5, 2.0], [0.0, 0.0, 0.8]) + + scene_cfg = BareSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + sim.reset() + + print("[INFO]: Setup complete...") + run_simulator(sim, scene, can_id_map, sock) + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/embedded/STM32/app/src/foc.cpp b/embedded/STM32/app/src/foc.cpp index 84185ffe..cd17b39a 100644 --- a/embedded/STM32/app/src/foc.cpp +++ b/embedded/STM32/app/src/foc.cpp @@ -1,5 +1,5 @@ -/** For setup +/** For setup * - initialize PWM * - initialize angle/position encoder (MT6835) * - intialize current sensing @@ -7,15 +7,15 @@ * - intialize motor configurations (PID, mode) + motor.initFOC() * - intialize CANFD and enable RX ISR * -*/ + */ -#include -#include #include "SimpleFOCDrivers.h" #include "encoders/mt6835/MagneticSensorMT6835.h" #include "stm32g4xx_hal.h" +#include +#include -BLDCMotor motor = BLDCMotor(7); +BLDCMotor motor = BLDCMotor(7); bool setup_success = true; float target_voltage = 0.0f; @@ -23,21 +23,14 @@ float target_voltage = 0.0f; #define M0_IN1_PIN PA_8 #define M0_IN2_PIN PA_9 #define M0_IN3_PIN PA_10 -#define M0_EN_PIN PB_5 +#define M0_EN_PIN PB_5 #define SENSOR_CS_PIN PB_6 BLDCDriver3PWM driver = BLDCDriver3PWM(M0_IN1_PIN, M0_IN2_PIN, M0_IN3_PIN, M0_EN_PIN); -SPISettings mt6835_spi_settings( - 1000000, - MT6835_BITORDER, - SPI_MODE3 -); +SPISettings mt6835_spi_settings(1000000, MT6835_BITORDER, SPI_MODE3); -MagneticSensorMT6835 sensor( - SENSOR_CS_PIN, - mt6835_spi_settings -); +MagneticSensorMT6835 sensor(SENSOR_CS_PIN, mt6835_spi_settings); FDCAN_HandleTypeDef hfdcan1; @@ -48,247 +41,208 @@ struct can_rx_message { FDCAN_RxHeaderTypeDef header; uint8_t data[64]; - }; - can_rx_message can_rx_queue[CAN_RX_QUEUE_SIZE]; volatile uint8_t can_rx_head = 0; volatile uint8_t can_rx_tail = 0; volatile uint32_t can_rx_dropped = 0; - bool initCANFD() { - /* - * Select PCLK1 as the FDCAN clock. - * - * The bit timings below assume PCLK1/FDCAN clock = 170 MHz. - */ - RCC_PeriphCLKInitTypeDef peripheral_clock = {}; - - peripheral_clock.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - peripheral_clock.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1; + /* + * Select PCLK1 as the FDCAN clock. + * + * The bit timings below assume PCLK1/FDCAN clock = 170 MHz. + */ + RCC_PeriphCLKInitTypeDef peripheral_clock = {}; + + peripheral_clock.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; + peripheral_clock.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1; + + if (HAL_RCCEx_PeriphCLKConfig(&peripheral_clock) != HAL_OK) { + return false; + } - if (HAL_RCCEx_PeriphCLKConfig(&peripheral_clock) != HAL_OK) - { - return false; - } + /* Enable peripheral and GPIO clocks. */ + __HAL_RCC_FDCAN_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + + /* + * PA11: FDCAN1_RX + * PA12: FDCAN1_TX + */ + GPIO_InitTypeDef gpio = {}; + + gpio.Pin = GPIO_PIN_11 | GPIO_PIN_12; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = GPIO_AF9_FDCAN1; + + HAL_GPIO_Init(GPIOA, &gpio); + + /* Configure the FDCAN peripheral. */ + hfdcan1.Instance = FDCAN1; + + hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1; + hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS; + hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + + hfdcan1.Init.AutoRetransmission = ENABLE; + hfdcan1.Init.TransmitPause = DISABLE; + hfdcan1.Init.ProtocolException = DISABLE; + + /* + * Nominal/arbitration rate: + * + * 170 MHz / [10 × (1 + 27 + 6)] + * = 500 kbit/s + */ + hfdcan1.Init.NominalPrescaler = 10; + hfdcan1.Init.NominalSyncJumpWidth = 6; + hfdcan1.Init.NominalTimeSeg1 = 27; + hfdcan1.Init.NominalTimeSeg2 = 6; + + /* + * Data-phase rate: + * + * 170 MHz / [5 × (1 + 13 + 3)] + * = 2 Mbit/s + */ + hfdcan1.Init.DataPrescaler = 5; + hfdcan1.Init.DataSyncJumpWidth = 3; + hfdcan1.Init.DataTimeSeg1 = 13; + hfdcan1.Init.DataTimeSeg2 = 3; + + hfdcan1.Init.StdFiltersNbr = 1; + hfdcan1.Init.ExtFiltersNbr = 0; + hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; + + if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) { + return false; + } - /* Enable peripheral and GPIO clocks. */ - __HAL_RCC_FDCAN_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); + /* + * Accept all 11-bit standard identifiers. + * + * Mask = 0 means no identifier bits need to match. + */ + FDCAN_FilterTypeDef filter = {}; + + filter.IdType = FDCAN_STANDARD_ID; + filter.FilterIndex = 0; + filter.FilterType = FDCAN_FILTER_MASK; + filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; + filter.FilterID1 = 0x000; + filter.FilterID2 = 0x000; + + if (HAL_FDCAN_ConfigFilter(&hfdcan1, &filter) != HAL_OK) { + return false; + } - /* - * PA11: FDCAN1_RX - * PA12: FDCAN1_TX - */ - GPIO_InitTypeDef gpio = {}; + /* + * Reject nonmatching messages and remote frames. + * All standard data frames match the filter above. + */ + if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, + FDCAN_REJECT_REMOTE) != HAL_OK) { + return false; + } - gpio.Pin = GPIO_PIN_11 | GPIO_PIN_12; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - gpio.Alternate = GPIO_AF9_FDCAN1; + /* Enable RX FIFO 0 new-message notification. */ + if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK) { + return false; + } - HAL_GPIO_Init(GPIOA, &gpio); + /* + * Recommended when transmitting FD frames using BRS. + * TDC offset = DataPrescaler × DataTimeSeg1 = 5 × 13. + */ + if (HAL_FDCAN_ConfigTxDelayCompensation(&hfdcan1, 65, 0) != HAL_OK) { + return false; + } - /* Configure the FDCAN peripheral. */ - hfdcan1.Instance = FDCAN1; + if (HAL_FDCAN_EnableTxDelayCompensation(&hfdcan1) != HAL_OK) { + return false; + } - hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1; - hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS; - hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + /* + * Enable the interrupt in the ARM NVIC. + * Priority 5 keeps it below very high-priority control interrupts. + */ + HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); - hfdcan1.Init.AutoRetransmission = ENABLE; - hfdcan1.Init.TransmitPause = DISABLE; - hfdcan1.Init.ProtocolException = DISABLE; + if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { + return false; + } - /* - * Nominal/arbitration rate: - * - * 170 MHz / [10 × (1 + 27 + 6)] - * = 500 kbit/s - */ - hfdcan1.Init.NominalPrescaler = 10; - hfdcan1.Init.NominalSyncJumpWidth = 6; - hfdcan1.Init.NominalTimeSeg1 = 27; - hfdcan1.Init.NominalTimeSeg2 = 6; + return true; +} - /* - * Data-phase rate: - * - * 170 MHz / [5 × (1 + 13 + 3)] - * = 2 Mbit/s - */ - hfdcan1.Init.DataPrescaler = 5; - hfdcan1.Init.DataSyncJumpWidth = 3; - hfdcan1.Init.DataTimeSeg1 = 13; - hfdcan1.Init.DataTimeSeg2 = 3; - - hfdcan1.Init.StdFiltersNbr = 1; - hfdcan1.Init.ExtFiltersNbr = 0; - hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; - - if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) - { - return false; - } +extern "C" void FDCAN1_IT0_IRQHandler() { + HAL_FDCAN_IRQHandler(&hfdcan1); +} - /* - * Accept all 11-bit standard identifiers. - * - * Mask = 0 means no identifier bits need to match. - */ - FDCAN_FilterTypeDef filter = {}; - - filter.IdType = FDCAN_STANDARD_ID; - filter.FilterIndex = 0; - filter.FilterType = FDCAN_FILTER_MASK; - filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; - filter.FilterID1 = 0x000; - filter.FilterID2 = 0x000; - - if (HAL_FDCAN_ConfigFilter(&hfdcan1, &filter) != HAL_OK) - { - return false; - } +extern "C" void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* hfdcan, + uint32_t rx_fifo0_interrupts) { + if (hfdcan->Instance != FDCAN1) { + return; + } - /* - * Reject nonmatching messages and remote frames. - * All standard data frames match the filter above. - */ - if (HAL_FDCAN_ConfigGlobalFilter( - &hfdcan1, - FDCAN_REJECT, - FDCAN_REJECT, - FDCAN_REJECT_REMOTE, - FDCAN_REJECT_REMOTE) != HAL_OK) - { - return false; - } + if ((rx_fifo0_interrupts & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U) { + return; + } - /* Enable RX FIFO 0 new-message notification. */ - if (HAL_FDCAN_ActivateNotification( - &hfdcan1, - FDCAN_IT_RX_FIFO0_NEW_MESSAGE, - 0) != HAL_OK) - { - return false; - } + /* + * Drain all currently available messages from hardware FIFO 0. + */ + while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0U) { + const uint8_t head = can_rx_head; + const uint8_t next = (head + 1U) & CAN_RX_QUEUE_MASK; /* - * Recommended when transmitting FD frames using BRS. - * TDC offset = DataPrescaler × DataTimeSeg1 = 5 × 13. + * Read into the current empty ring-buffer entry. */ - if (HAL_FDCAN_ConfigTxDelayCompensation( - &hfdcan1, - 65, - 0) != HAL_OK) - { - return false; - } - - if (HAL_FDCAN_EnableTxDelayCompensation(&hfdcan1) != HAL_OK) - { - return false; + if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &can_rx_queue[head].header, + can_rx_queue[head].data) != HAL_OK) { + break; } /* - * Enable the interrupt in the ARM NVIC. - * Priority 5 keeps it below very high-priority control interrupts. + * If next == tail, the software queue is full. + * The hardware message has still been drained, but it is discarded. */ - HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); - - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) - { - return false; - } - - return true; -} - - -extern "C" void FDCAN1_IT0_IRQHandler() -{ - HAL_FDCAN_IRQHandler(&hfdcan1); -} - -extern "C" void HAL_FDCAN_RxFifo0Callback( - FDCAN_HandleTypeDef *hfdcan, - uint32_t rx_fifo0_interrupts) -{ - if (hfdcan->Instance != FDCAN1) - { - return; - } - - if ((rx_fifo0_interrupts & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U) - { - return; + if (next == can_rx_tail) { + can_rx_dropped++; + continue; } /* - * Drain all currently available messages from hardware FIFO 0. + * Ensure message data is written before publishing the new head. */ - while (HAL_FDCAN_GetRxFifoFillLevel( - hfdcan, - FDCAN_RX_FIFO0) > 0U) - { - const uint8_t head = can_rx_head; - const uint8_t next = - (head + 1U) & CAN_RX_QUEUE_MASK; - - /* - * Read into the current empty ring-buffer entry. - */ - if (HAL_FDCAN_GetRxMessage( - hfdcan, - FDCAN_RX_FIFO0, - &can_rx_queue[head].header, - can_rx_queue[head].data) != HAL_OK) - { - break; - } - - /* - * If next == tail, the software queue is full. - * The hardware message has still been drained, but it is discarded. - */ - if (next == can_rx_tail) - { - can_rx_dropped++; - continue; - } - - /* - * Ensure message data is written before publishing the new head. - */ - __DMB(); - can_rx_head = next; - } + __DMB(); + can_rx_head = next; + } } -bool popCANMessage(can_rx_message &message) -{ - const uint8_t tail = can_rx_tail; +bool popCANMessage(can_rx_message& message) { + const uint8_t tail = can_rx_tail; - if (tail == can_rx_head) - { - return false; - } + if (tail == can_rx_head) { + return false; + } - message = can_rx_queue[tail]; + message = can_rx_queue[tail]; - __DMB(); + __DMB(); - can_rx_tail = (tail + 1U) & CAN_RX_QUEUE_MASK; + can_rx_tail = (tail + 1U) & CAN_RX_QUEUE_MASK; - return true; + return true; } - /* * Example protocol: * @@ -302,52 +256,40 @@ bool popCANMessage(can_rx_message &message) * 1000 -> +1.000 V * -500 -> -0.500 V */ -void processCANMessage(const can_rx_message &message) -{ - if (message.header.IdType != FDCAN_STANDARD_ID) - { - return; - } +void processCANMessage(const can_rx_message& message) { + if (message.header.IdType != FDCAN_STANDARD_ID) { + return; + } - if (message.header.Identifier != 0x100) - { - return; - } + if (message.header.Identifier != 0x100) { + return; + } - if (message.header.DataLength != FDCAN_DLC_BYTES_2) - { - return; - } + if (message.header.DataLength != FDCAN_DLC_BYTES_2) { + return; + } - int16_t target_millivolts = - static_cast( - static_cast(message.data[0]) | - (static_cast(message.data[1]) << 8U) - ); + int16_t target_millivolts = static_cast(static_cast(message.data[0]) | + (static_cast(message.data[1]) << 8U)); - float requested_voltage = - static_cast(target_millivolts) / 1000.0f; + float requested_voltage = static_cast(target_millivolts) / 1000.0f; - /* Clamp the command to the configured safety limit. */ - if (requested_voltage > motor.voltage_limit) - { - requested_voltage = motor.voltage_limit; - } - else if (requested_voltage < -motor.voltage_limit) - { - requested_voltage = -motor.voltage_limit; - } + /* Clamp the command to the configured safety limit. */ + if (requested_voltage > motor.voltage_limit) { + requested_voltage = motor.voltage_limit; + } else if (requested_voltage < -motor.voltage_limit) { + requested_voltage = -motor.voltage_limit; + } - target_voltage = requested_voltage; + target_voltage = requested_voltage; } -void setup() -{ +void setup() { Serial.begin(115200); SimpleFOCDebug::enable(&Serial); - + // need to check with motor driver specs driver.voltage_power_supply = 12.0; // used for initial testing @@ -355,7 +297,7 @@ void setup() // might need to be changed driver.pwm_frequency = 20000; - if (!driver.init()){ + if (!driver.init()) { Serial.println("Driver init failed!"); setup_success = false; return; @@ -372,7 +314,7 @@ void setup() motor.torque_controller = TorqueControlType::voltage; motor.init(); - if (!motor.initFOC()){ + if (!motor.initFOC()) { Serial.println("Motor init failed!"); setup_success = false; return; @@ -384,27 +326,24 @@ void setup() setup_success = false; return; } - + Serial.println("PWM, driver, and motor initialized."); delay(1000); - } - /** For loop * -call loopFOC, move * -check ringbuffer for messages * -send telemetry data back - * + * */ -void loop() -{ +void loop() { if (!setup_success) { return; } can_rx_message message; - if (popCANMessage(message)){ + if (popCANMessage(message)) { processCANMessage(message); }