diff --git a/docs/developer-docs/Wago-Stepper-Documentation.md b/docs/developer-docs/Wago-Stepper-Documentation.md new file mode 100644 index 00000000..a7bc8fd3 --- /dev/null +++ b/docs/developer-docs/Wago-Stepper-Documentation.md @@ -0,0 +1,931 @@ +# Wago Stepper Controller - Developer Documentation + +**Models:** 750-671 & 750-672 +**Last Updated:** February 2026 + +## Table of Contents + +1. [Overview](#overview) +2. [Hardware Quick Reference](#hardware-quick-reference) +3. [Architecture & Code Organization](#architecture--code-organization) +4. [Control & Status Bytes](#control--status-bytes) +5. [Implementation Details](#implementation-details) +6. [Prescalers & Calculations](#prescalers--calculations) +7. [Common Pitfalls & Debug Tips](#common-pitfalls--debug-tips) + +--- + +## Overview + +The Wago Stepper Controllers (750-671 and 750-672) drive stepper motors via EtherCAT. This documentation focuses on **velocity/speed control mode** implementation. + +### Key Differences Between Models + +| Feature | 750-671 | 750-672 | +| --------------- | ------------------------------------------ | ------------------------------------ | +| Max Voltage | 50 VDC | 70 VDC | +| Max Current | 5 A | 7.5 A | +| Speed Control | Via Application Selector in config | Via Control Byte C1 command | +| Control Byte C1 | Different bit layout (M_Speed_Control bit) | Command field (0x18 = Speed Control) | + +**For 750-671:** Must set `Application_Selector = 2` and `PWM_Period = 0` in WAGO I/O CHECK before using speed control. + +**For 750-672:** Speed control is selected directly via command in Control Byte C1 (no config needed). + +### Process Image + +Both models use **12 bytes bidirectional** (96 bits each way): + +``` +Input (TxPDO - Controller → PLC): + [0] S0 (status) + [1] Reserved + [2-3] Actual Velocity (i16) + [4-5] Reserved + [6-8] Actual Position (24-bit: L, M, H) + [9] S3 (digital inputs, warnings) + [10] S2 (busy, error, on_speed, etc.) + [11] S1 (acknowledgments) + +Output (RxPDO - PLC → Controller): + [0] C0 (control) + [1] Reserved + [2-3] Target Velocity (i16, clamped to ±25000) + [4-5] Acceleration (u16) + [6-8] Position (24-bit: L, M, H) + [9] C3 (position/reset control) + [10] C2 (prescalers, error quit) + [11] C1 (main control: enable, start, mode) +``` + +--- + +## Hardware Quick Reference + +### Connections + +| Pin | Name | Purpose | +|-----|------|---------| +| DI1+ | Digital Input 1 | Enable input (connect to 24V or control via software) | +| DI2+ | Digital Input 2 | Referencing input | +| DI- | Ground | Reference potential for DI1/DI2 | +| A1, A2 | Motor Winding A | Connect to motor phase A | +| B1, B2 | Motor Winding B | Connect to motor phase B | + +### Status LEDs + +- **LED H:** Shows error codes via blink patterns (see Wago manual section 3.3 for error codes) +- Other LEDs show Ready, Enable, Busy states (see manual section 2.1.2.3) + +--- + +## Architecture & Code Organization + +### File Structure + +``` +devices/ + wago_modules/ + wago_750_672.rs # Device struct, EtherCAT interface + +io/ + stepper_velocity_wago_750_672.rs # High-level wrapper & byte helpers +``` + +### Key Components + +#### 1. `Wago750_672` Struct (EtherCAT Device) + +Located in `devices/wago_modules/wago_750_672.rs` + +```rust +pub struct Wago750_672 { + is_used: bool, + tx_bit_offset: usize, // Input data offset + rx_bit_offset: usize, // Output data offset + pub rxpdo: Wago750_672RxPdo, // Data we send + pub txpdo: Wago750_672TxPdo, // Data we receive + module: Option, + pub state: InitState, + pub initialized: bool, +} +``` + +**Responsibilities:** +- Implements EtherCAT device traits +- Handles bit-level I/O with process image +- Runs state machine in `input()` method +- Thread-safe (used via `Arc>`) + +#### 2. `StepperVelocityWago750672` Wrapper + +Located in `io/stepper_velocity_wago_750_672.rs` + +```rust +pub struct StepperVelocityWago750672 { + pub device: Arc>, + pub state: InitState, + pub target_velocity: i16, + pub target_acceleration: u16, + pub enabled: bool, + pub freq_range_sel: u8, + pub acc_range_sel: u8, +} +``` + +**Responsibilities:** +- Provides high-level API (`set_velocity()`, `set_enabled()`) +- Manages prescaler settings +- Abstracts away state machine complexity + +### Why Two Layers? + +1. **`Wago750_672`:** Low-level EtherCAT device that handles cycle-by-cycle I/O +2. **`StepperVelocityWago750672`:** High-level wrapper for application logic + +This separation allows: +- EtherCAT device to run in tight real-time loop +- Application code to use simple API without worrying about cycles/acknowledgments +- Easy testing of each layer independently + +--- + +## Control & Status Bytes + +### Design Pattern: Type-Safe Byte Construction + +Instead of hardcoding hex values like `0x1B`, we use builder pattern with enums: + +```rust +// ❌ BAD - magic numbers, error-prone +dev.rxpdo.c1 = 0x1B; + +// ✅ GOOD - explicit, self-documenting +let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_command(C1Command::SpeedControl) + .bits(); +dev.rxpdo.c1 = c1; +``` + +### Wago 750-672 Control Bytes + +#### Control Byte C1 (Offset 11) - Main Control + +```rust +pub struct ControlByteC1(u8); + +pub enum C1Flag { + Enable = 0b0000_0001, // Bit 0: Must be 1 to enable + Stop2N = 0b0000_0010, // Bit 1: Must be 1 for normal operation + Start = 0b0000_0100, // Bit 2: Rising edge triggers action +} + +pub enum C1Command { + Idle = 0b0000_0000, // Bits 3-7: Operating mode + SinglePosition = 0b0000_1000, + RunProgram = 0b0001_0000, + SpeedControl = 0b0001_1000, // ← We use this + Reference = 0b0010_0000, + JogMode = 0b0010_1000, + Mailbox = 0b0011_0000, +} +``` + +**Implementation:** + +```rust +impl ControlByteC1 { + pub const fn new() -> Self { + Self(0) + } + + pub const fn with_flag(mut self, flag: C1Flag) -> Self { + self.0 |= flag as u8; // OR in the bit + self + } + + pub const fn with_command(mut self, cmd: C1Command) -> Self { + self.0 = (self.0 & 0b0000_0111) | (cmd as u8); // Mask lower 3 bits, set upper 5 + self + } + + pub const fn bits(self) -> u8 { + self.0 + } +} +``` + +**Key Points:** +- `const fn` allows compile-time evaluation +- Chainable builder pattern +- Command overwrites bits 3-7 while preserving 0-2 +- Flags are additive (multiple can be set) + +#### Control Byte C2 (Offset 10) - Prescalers & Error Handling + +```rust +pub struct ControlByteC2(u8); + +pub enum C2Flag { + FreqRangeSelL = 0b0000_0001, // Bit 0: Freq prescaler LSB + FreqRangeSelH = 0b0000_0010, // Bit 1: Freq prescaler MSB + AccRangeSelL = 0b0000_0100, // Bit 2: Acc prescaler LSB + AccRangeSelH = 0b0000_1000, // Bit 3: Acc prescaler MSB + PreCalc = 0b0100_0000, // Bit 6: Pre-calculation + ErrorQuit = 0b1000_0000, // Bit 7: Acknowledge error +} + +impl ControlByteC2 { + pub const fn with_freq_range(mut self, sel: u8) -> Self { + self.0 = (self.0 & 0b1111_1100) | (sel & 0b0000_0011); + self + } + + pub const fn with_acc_range(mut self, sel: u8) -> Self { + self.0 = (self.0 & 0b1111_0011) | ((sel & 0b0000_0011) << 2); + self + } +} +``` + +**Critical:** Prescalers can only be changed when controller is **disabled** (before Enable state). + +#### Control Byte C3 (Offset 9) - Position & Reset + +```rust +pub struct ControlByteC3(u8); + +pub enum C3Flag { + SetActualPos = 0b0000_0001, // Bit 0: Set current position + DirectionPos = 0b0000_0010, // Bit 2: Force positive direction + DirectionNeg = 0b0000_0100, // Bit 3: Force negative direction + ResetQuit = 0b1000_0000, // Bit 7: Acknowledge reset +} +``` + +### Status Bytes (Read-Only) + +#### Status Byte S1 (Offset 11) - Acknowledgments + +```rust +pub struct StatusByteS1(u8); + +pub enum S1Flag { + Ready = 0b0000_0001, // Bit 0: Acknowledges Enable + Stop2NAck = 0b0000_0010, // Bit 1: Acknowledges Stop2_N + StartAck = 0b0000_0100, // Bit 2: Acknowledges Start +} +// Bits 3-7: Mirrors C1 command when acknowledged + +impl StatusByteS1 { + pub const fn has_flag(self, flag: S1Flag) -> bool { + (self.0 & flag as u8) != 0 + } +} +``` + +**Usage Pattern:** +```rust +// Wait for acknowledgment +if self.txpdo.s1 == c1 { + // All bits acknowledged, proceed +} +``` + +#### Status Byte S2 (Offset 10) - Operation Status + +```rust +pub struct StatusByteS2(u8); + +pub enum S2Flag { + OnTarget = 0b0000_0001, // Bit 0: Position reached (positioning mode) + Busy = 0b0000_0010, // Bit 1: Executing command + StandStill = 0b0000_0100, // Bit 2: Motor not moving + OnSpeed = 0b0000_1000, // Bit 3: Target velocity reached + Direction = 0b0001_0000, // Bit 4: Rotation direction (0=neg, 1=pos) + ReferenceOk = 0b0010_0000, // Bit 5: Referencing complete + PreCalcAck = 0b0100_0000, // Bit 6: Pre-calc acknowledged + Error = 0b1000_0000, // Bit 7: Error occurred (check LED H) +} +``` + +**Critical for velocity control:** Monitor `OnSpeed` bit to know when target velocity is reached. + +#### Status Byte S3 (Offset 9) - Digital Inputs & Warnings + +```rust +pub struct StatusByteS3(u8); + +pub enum S3Flag { + Input1 = 0b0000_0001, // Bits 0-5: Digital input states + Input2 = 0b0000_0010, + Input3 = 0b0000_0100, + Input4 = 0b0000_1000, + Input5 = 0b0001_0000, + Input6 = 0b0010_0000, + Warning = 0b0100_0000, // Bit 6: Warning condition + Reset = 0b1000_0000, // Bit 7: Reset requested +} +``` + +--- + +## Implementation Details + +### Critical Initialization Sequence + +⚠️ **This sequence MUST be followed exactly or the controller won't work:** + +1. **Set non-zero acceleration** (before enabling) +2. **Set Enable + Stop2_N** (wait for ack) +3. **Set operating mode** (0x18 for speed control, wait for ack) +4. **Issue Start pulse** (set to 1, wait for ack, set to 0, wait for ack clear) +5. **Running** (repeat step 4 every time velocity changes) + +### State Machine (Why It Exists) + +The state machine exists because **acknowledgments happen in different EtherCAT cycles**. You can't just set everything in one cycle. + +```rust +#[derive(Debug, Clone)] +pub enum InitState { + Off, // Controller disabled + Enable, // Setting Enable + Stop2_N, waiting for ack + SetMode, // Setting speed control mode, waiting for ack + StartPulseStart, // Setting Start bit, waiting for ack + StartPulseEnd, // Clearing Start bit, waiting for ack clear + Running, // Normal operation + ErrorQuit, // Handling error + ResetQuit, // Handling reset +} +``` + +### State Machine Implementation + +Located in `Wago750_672::input()` method (runs every EtherCAT cycle): + +```rust +fn input(&mut self, input: &BitSlice) -> Result<(), anyhow::Error> { + // 1. Read process image from controller + let base = self.tx_bit_offset; + let mut b = [0u8; 12]; + for i in 0..12 { + b[i] = input[base + i * 8..base + (i + 1) * 8].load_le(); + } + + self.txpdo = Wago750_672TxPdo { + s0: b[0], + actual_velocity: i16::from_le_bytes([b[2], b[3]]), + position_l: b[6], + position_m: b[7], + position_h: b[8], + s3: b[9], + s2: b[10], + s1: b[11], + }; + + // 2. State machine logic + match self.state { + InitState::Off => { + self.initialized = false; + // Do nothing until external enable request + } + + InitState::Enable => { + // Build C1 byte with Enable and Stop2_N + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .bits(); + self.rxpdo.c1 = c1; + + // Wait for acknowledgment + if self.txpdo.s1 == c1 { + self.state = InitState::SetMode; + } + } + + InitState::SetMode => { + // Add speed control command + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.c1 = c1; + + // Wait for mode acknowledgment + if self.txpdo.s1 == c1 { + self.initialized = true; + self.state = InitState::StartPulseStart; + } + } + + InitState::StartPulseStart => { + // Set Start bit + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_flag(C1Flag::Start) // ← Rising edge + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.c1 = c1; + + // Wait for Start acknowledgment + if self.txpdo.s1 == c1 { + self.state = InitState::StartPulseEnd; + } + } + + InitState::StartPulseEnd => { + // Clear Start bit + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.c1 = c1; + + // Wait for acknowledgment to clear + if self.txpdo.s1 == c1 { + self.state = InitState::Running; + } + } + + InitState::Running => { + // Monitor for errors and resets + let c2_error_mask = ControlByteC2::new().with_flag(C2Flag::ErrorQuit).bits(); + let c3_reset_mask = ControlByteC3::new().with_flag(C3Flag::ResetQuit).bits(); + + if self.txpdo.s2 & c2_error_mask != 0 { + self.state = InitState::ErrorQuit; + } else if self.txpdo.s3 & c3_reset_mask != 0 { + self.state = InitState::ResetQuit; + } + } + + InitState::ErrorQuit => { + // Set Error_Quit bit to acknowledge + self.rxpdo.c2 |= ControlByteC2::new().with_flag(C2Flag::ErrorQuit).bits(); + tracing::error!("Stepper Controller Errored. Trying to reenable..."); + self.state = InitState::Enable; // Restart initialization + } + + InitState::ResetQuit => { + // Set Reset_Quit bit to acknowledge + self.rxpdo.c3 |= ControlByteC3::new().with_flag(C3Flag::ResetQuit).bits(); + tracing::error!("Stepper Controller Reset. Trying to reenable..."); + self.state = InitState::Enable; // Restart initialization + } + } + + Ok(()) +} +``` + +**Key Implementation Notes:** + +1. **State changes happen AFTER acknowledgment:** We wait for `self.txpdo.s1 == c1` before moving to next state +2. **Enable and Stop2_N stay high:** They're included in every C1 byte after Enable state +3. **Start pulse is a rising edge:** Must go 1 → 0, not just stay at 1 +4. **Error recovery:** Automatically attempts to re-enable on error/reset + +### High-Level API Implementation + +Located in `StepperVelocityWago750672`: + +```rust +pub fn set_enabled(&mut self, enabled: bool) { + // Prevent redundant enables + if self.enabled && enabled { + return; + } + + // Safety check: acceleration must be non-zero + if self.get_target_acceleration() == 0 { + return; + } + + self.enabled = enabled; + if enabled { + self.change_init_state(InitState::Enable); + } else { + self.change_init_state(InitState::Off); + self.write_control_byte(ControlByte::C1, 0b00000000); + } +} + +pub fn set_velocity(&mut self, velocity: i16) { + self.target_velocity = velocity; + + let mut dev = block_on(self.device.write()); + + // Clamp to valid range + dev.rxpdo.velocity = velocity.clamp(-25000, 25000); + + // Trigger Start pulse if already initialized + if dev.initialized { + dev.state = InitState::StartPulseStart; + } +} + +pub fn set_acceleration(&mut self, acceleration: u16) { + self.target_acceleration = acceleration; + + let mut dev = block_on(self.device.write()); + dev.rxpdo.acceleration = acceleration; +} +``` + +**Why the wrapper?** +- Application code doesn't need to know about state machine +- Handles common errors (zero acceleration check) +- Thread-safe access to device +- Clamping and validation in one place + +### Changing Velocity at Runtime + +⚠️ **Critical:** Must issue a **new Start pulse** for every velocity change: + +```rust +// User calls this +stepper.set_velocity(15000); + +// Internally, set_velocity() does: +dev.rxpdo.velocity = 15000; +if dev.initialized { + dev.state = InitState::StartPulseStart; // ← Triggers new pulse +} + +// State machine then handles: +// StartPulseStart → StartPulseEnd → Running +// (Takes 2-3 EtherCAT cycles) +``` + +**Common mistake:** Setting velocity without triggering Start pulse - motor won't change speed. + +--- + +## Prescalers & Calculations + +Prescalers scale the velocity and acceleration values. They **can only be changed when disabled**. + +### Frequency Prescaler (Bits 0-1 of C2) + +Controls velocity scaling: + +| Freq_Range_Sel | Prescaler | Max Frequency | Resolution | +|----------------|-----------|---------------|------------| +| 00 | 200 (default) | 10 kHz | Configurable | +| 01 | 80 | 25 kHz | 1 Hz | +| 10 | 20 | 100 kHz | 20 Hz | +| 11 | 4 | 500 kHz | 20 Hz | + +**Formula:** +``` +Output Frequency (fp) = Velocity × 80 / Freq_Prescaler [Hz] +``` + +**Example:** +```rust +// Freq_Range_Sel = 01 (prescaler = 80) +// Velocity = 10000 +// Output: 10000 × 80 / 80 = 10000 Hz + +stepper.set_freq_range_sel(1); // Must be done BEFORE enable +stepper.set_velocity(10000); +``` + +### Acceleration Prescaler (Bits 2-3 of C2) + +Controls acceleration scaling: + +| Acc_Range_Sel | Multiplier | Time to Max Speed | +|---------------|------------|-------------------| +| 00 | 8 (default) | 7600 ms | +| 01 | 80 | 760 ms | +| 10 | 800 | 76 ms | +| 11 | 8000 | 7.6 ms | + +**Formula:** +``` +Acceleration (a) = Acceleration_Value × Acc_Multiplier / Freq_Prescaler [Hz/s] +``` + +**Example:** +```rust +// Acc_Range_Sel = 01 (multiplier = 80) +// Freq_Range_Sel = 01 (prescaler = 80) +// Acceleration_Value = 10000 +// Output: 10000 × 80 / 80 = 10000 Hz/s + +stepper.set_acc_range_sel(1); // Must be done BEFORE enable +stepper.set_acceleration(10000); +``` + +### Setting Prescalers (Implementation) + +```rust +pub fn set_freq_range_sel(&mut self, factor: u8) { + // Safety checks + if self.enabled || factor > 3 { + return; // Can't change while enabled + } + + self.freq_range_sel = factor; + + let mut dev = block_on(self.device.write()); + let c2 = ControlByteC2::from_bits(dev.rxpdo.c2) + .with_freq_range(factor) + .bits(); + dev.rxpdo.c2 = c2; +} + +pub fn set_acc_range_sel(&mut self, factor: u8) { + if self.enabled || factor > 3 { + return; + } + + self.acc_range_sel = factor; + + let mut dev = block_on(self.device.write()); + let c2 = ControlByteC2::from_bits(dev.rxpdo.c2) + .with_acc_range(factor) + .bits(); + dev.rxpdo.c2 = c2; +} +``` + +**Usage:** +```rust +let mut stepper = StepperVelocityWago750672::new(device); + +// Set prescalers FIRST (before enabling) +stepper.set_freq_range_sel(1); +stepper.set_acc_range_sel(1); +stepper.set_acceleration(10000); + +// Then enable +stepper.set_enabled(true); + +// Now can set velocity +stepper.set_velocity(15000); +``` + +--- + +## Common Pitfalls & Debug Tips + +### 1. Controller Won't Enable + +**Symptoms:** +- State stuck in `Enable` +- `initialized` never becomes true +- Motor doesn't move + +**Debug checklist:** +```rust +// Check acceleration is non-zero +println!("Acceleration: {}", dev.rxpdo.acceleration); // Must be > 0 + +// Check DI1 if hardware enable is configured +println!("DI1 state: {}", (dev.txpdo.s3 & 0x01) != 0); // Should be 1 + +// Check status byte acknowledgment +println!("C1: {:#010b}", dev.rxpdo.c1); +println!("S1: {:#010b}", dev.txpdo.s1); // Should match C1 +``` + +**Common causes:** +```rust +// ❌ Forgot to set acceleration +stepper.set_acceleration(0); // Won't enable! + +// ✅ Always set non-zero acceleration +stepper.set_acceleration(10000); +stepper.set_enabled(true); +``` + +### 2. Velocity Won't Change + +**Symptoms:** + - Motor running but won't change speed +- `set_velocity()` called but no effect + +**Debug:** +```rust +// Check initialized flag +println!("Initialized: {}", dev.initialized); + +// Check state after velocity change +println!("State: {:?}", dev.state); // Should become StartPulseStart + +// Manually trigger Start pulse (bypass wrapper) +dev.state = InitState::StartPulseStart; +``` + +**Common causes:** +```rust +// ❌ Setting velocity before initialization +stepper.set_velocity(10000); // Controller not ready yet + +// ✅ Check initialized first +if stepper.device.read().initialized { + stepper.set_velocity(10000); +} +``` + +### 3. Prescaler Not Working + +**Symptoms:** +- Velocity calculation seems wrong +- Can't reach expected frequency + +**Debug:** +```rust +// Check prescalers were set BEFORE enabling +println!("Freq prescaler: {}", stepper.freq_range_sel); +println!("Acc prescaler: {}", stepper.acc_range_sel); +println!("Enabled: {}", stepper.enabled); + +// Check C2 byte +let dev = stepper.device.read(); +println!("C2: {:#010b}", dev.rxpdo.c2); +// Bits 0-1: freq, Bits 2-3: acc +``` + +**Common causes:** +```rust +// ❌ Setting prescaler after enable +stepper.set_enabled(true); +stepper.set_freq_range_sel(1); // Too late! Ignored. + +// ✅ Set prescalers FIRST +stepper.set_freq_range_sel(1); +stepper.set_acc_range_sel(1); +stepper.set_enabled(true); +``` + +### 4. State Machine Stuck + +**Symptoms:** +- State doesn't progress +- Acknowledgments not received + +**Debug:** +```rust +// Add logging to state machine +match self.state { + InitState::Enable => { + println!("Enable state: C1={:#010b}, S1={:#010b}", self.rxpdo.c1, self.txpdo.s1); + // ... + } +} + +// Check EtherCAT communication +// - Is cycle running? +// - Are other devices working? +// - Check bus topology +``` + +### 5. Error Bit Set + +**Symptoms:** +- Error bit (S2 bit 7) is 1 +- LED H blinking +- Controller stops + +**Debug:** +```rust +// Check error state +let s2 = StatusByteS2::from_bits(dev.txpdo.s2); +if s2.has_flag(S2Flag::Error) { + println!("ERROR DETECTED"); + + // Count LED H blinks for error code + // See Wago manual section 3.3 + + // Check other status flags for clues + println!("Busy: {}", s2.has_flag(S2Flag::Busy)); + println!("StandStill: {}", s2.has_flag(S2Flag::StandStill)); +} +``` + +**Common error codes:** +- **2 blinks:** Overcurrent +- **3 blinks:** Position error +- **4 blinks:** Encoder fault +- See Wago manual for complete list + +--- + +### Supporting Wago 750-671 + +The 750-671 uses different C1 bit layout: + +1. **Create separate enum:** +```rust +// For 750-671 +pub enum C1Flag671 { + Enable = 0b0000_0001, // Bit 0 + Stop2N = 0b0000_0010, // Bit 1 + Start = 0b0000_0100, // Bit 2 + MSpeedControl = 0b0000_1000, // Bit 3 (instead of command field) + MProgram = 0b0001_0000, // Bit 4 + MReference = 0b0010_0000, // Bit 5 + MJog = 0b0100_0000, // Bit 6 + MDriveByMBX = 0b1000_0000, // Bit 7 +} +``` + +--- + +## Quick Reference Card + +### Initialization Checklist +```rust +// 1. Create and configure +let stepper = StepperVelocityWago750672::new(device); + +// 2. Set prescalers (optional, before enable) +stepper.set_freq_range_sel(1); // 0-3 +stepper.set_acc_range_sel(1); // 0-3 + +// 3. Set non-zero acceleration (REQUIRED) +stepper.set_acceleration(10000); + +// 4. Enable +stepper.set_enabled(true); + +// 5. Set velocity (after initialized) +stepper.set_velocity(15000); // ±25000 max +``` + +### Key Constants +```rust +const SPEED_CONTROL_CMD: u8 = 0x18; // C1 command for 750-672 +const MAX_VELOCITY: i16 = 25000; +const MIN_VELOCITY: i16 = -25000; +const DEFAULT_ACCELERATION: u16 = 10000; +``` + +### Critical Rules +1. ⚠️ **Never enable with zero acceleration** +2. ⚠️ **Always wait for acknowledgments** +3. ⚠️ **Issue Start pulse for every velocity change** +4. ⚠️ **Prescalers can only be changed when disabled** +5. ⚠️ **Keep Enable and Stop2_N high during operation** + +### Common Bit Patterns +```rust +// Enable + Stop2_N +0b0000_0011 + +// Enable + Stop2_N + Speed Control +0b0001_1011 + +// Enable + Stop2_N + Speed Control + Start +0b0001_1111 +``` + +### Debug Commands +```bash +# View byte values +println!("C1: {:#010b}", dev.rxpdo.c1); +println!("S1: {:#010b}", dev.txpdo.s1); + +# Check specific flags +let s2 = StatusByteS2::from_bits(dev.txpdo.s2); +println!("Error: {}", s2.has_flag(S2Flag::Error)); +println!("OnSpeed: {}", s2.has_flag(S2Flag::OnSpeed)); + +# Monitor state +println!("State: {:?}", dev.state); +println!("Initialized: {}", dev.initialized); +``` + +--- + +## Additional Resources + +- **Wago Manual:** Section 2.1.2.3 (LEDs), 3.3 (Error Codes) +- **WAGO I/O CHECK:** Configuration software for 750-671 application selector +- **EtherCAT Specification:** www.ethercat.org +- **Code Location:** + - Device: `devices/wago_modules/wago_750_672.rs` + - Wrapper: `io/stepper_velocity_wago_750_672.rs` + +--- + +## Changelog + +| Version | Date | Changes | +| ------- | ------- | ------------------------------------------- | +| 1.0 | 2025 | Initial implementation with state machine | +| 2.0 | 2026-02 | Added comprehensive developer documentation | + +--- + +**Need help?** Check the troubleshooting section or add debug logging using the template above. \ No newline at end of file diff --git a/electron/package-lock.json b/electron/package-lock.json index 80b01511..a69a444a 100644 --- a/electron/package-lock.json +++ b/electron/package-lock.json @@ -162,7 +162,6 @@ "resolved": "https://registry.npmjs.org/@babel/core/-/core-7.28.5.tgz", "integrity": "sha512-e7jT4DxYvIDLk1ZHmU/m/mB19rex9sv0c2ftBtjSBv+kVM/902eh0fINUzD7UwLLNR+jU585GxUJ8/EBfAM5fw==", "license": "MIT", - "peer": true, "dependencies": { "@babel/code-frame": "^7.27.1", "@babel/generator": "^7.28.5", @@ -659,7 +658,6 @@ } ], "license": "MIT", - "peer": true, "engines": { "node": ">=18" }, @@ -683,7 +681,6 @@ } ], "license": "MIT", - "peer": true, "engines": { "node": ">=18" } @@ -3138,66 +3135,6 @@ "node": ">=14.0.0" } }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/@emnapi/core": { - "version": "1.7.1", - "dev": true, - "inBundle": true, - "license": "MIT", - "optional": true, - "dependencies": { - "@emnapi/wasi-threads": "1.1.0", - "tslib": "^2.4.0" - } - }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/@emnapi/runtime": { - "version": "1.7.1", - "dev": true, - "inBundle": true, - "license": "MIT", - "optional": true, - "dependencies": { - "tslib": "^2.4.0" - } - }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/@emnapi/wasi-threads": { - "version": "1.1.0", - "dev": true, - "inBundle": true, - "license": "MIT", - "optional": true, - "dependencies": { - "tslib": "^2.4.0" - } - }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/@napi-rs/wasm-runtime": { - "version": "1.1.0", - "dev": true, - "inBundle": true, - "license": "MIT", - "optional": true, - "dependencies": { - "@emnapi/core": "^1.7.1", - "@emnapi/runtime": "^1.7.1", - "@tybys/wasm-util": "^0.10.1" - } - }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/@tybys/wasm-util": { - "version": "0.10.1", - "dev": true, - "inBundle": true, - "license": "MIT", - "optional": true, - "dependencies": { - "tslib": "^2.4.0" - } - }, - "node_modules/@tailwindcss/oxide-wasm32-wasi/node_modules/tslib": { - "version": "2.8.1", - "dev": true, - "inBundle": true, - "license": "0BSD", - "optional": true - }, "node_modules/@tailwindcss/oxide-win32-arm64-msvc": { "version": "4.1.18", "resolved": "https://registry.npmjs.org/@tailwindcss/oxide-win32-arm64-msvc/-/oxide-win32-arm64-msvc-4.1.18.tgz", @@ -3290,7 +3227,6 @@ "resolved": "https://registry.npmjs.org/@tanstack/react-router/-/react-router-1.141.1.tgz", "integrity": "sha512-pLQ6ZFCh5s86ewZIAu2wQc2svf+DqttD7CFd1NPSxdEU20KvMMj9RPQdDHj/pbf2VvB3i1nEDxjli/Z0PBvnCQ==", "license": "MIT", - "peer": true, "dependencies": { "@tanstack/history": "1.141.0", "@tanstack/react-store": "^0.8.0", @@ -3381,7 +3317,6 @@ "resolved": "https://registry.npmjs.org/@tanstack/router-core/-/router-core-1.141.1.tgz", "integrity": "sha512-fR1GGpp6v3dVKu4KIAjEh+Sd0qGLQd/wvCOVHeopSY6aFidXKCzwrS5cBOBqoPPWTKmn6CdW1a0CzFr5Furdog==", "license": "MIT", - "peer": true, "dependencies": { "@tanstack/history": "1.141.0", "@tanstack/store": "^0.8.0", @@ -3602,7 +3537,8 @@ "resolved": "https://registry.npmjs.org/@types/aria-query/-/aria-query-5.0.4.tgz", "integrity": "sha512-rfT93uj5s0PRL7EzccGMs3brplhcrghnDoV26NqKhCAS1hVo+WdNsPvE/yb6ilfr5hi2MEk6d5EWJTKdxg8jVw==", "dev": true, - "license": "MIT" + "license": "MIT", + "peer": true }, "node_modules/@types/babel__core": { "version": "7.20.5", @@ -3768,7 +3704,6 @@ "integrity": "sha512-gqkrWUsS8hcm0r44yn7/xZeV1ERva/nLgrLxFRUGb7aoNMIJfZJ3AC261zDQuOAKC7MiXai1WCpYc48jAHoShQ==", "devOptional": true, "license": "MIT", - "peer": true, "dependencies": { "undici-types": "~7.16.0" } @@ -3785,7 +3720,6 @@ "resolved": "https://registry.npmjs.org/@types/react/-/react-19.2.7.tgz", "integrity": "sha512-MWtvHrGZLFttgeEj28VXHxpmwYbor/ATPYbBfSFZEIRK0ecCFLl2Qo55z52Hss+UV9CRN7trSeq1zbgx7YDWWg==", "license": "MIT", - "peer": true, "dependencies": { "csstype": "^3.2.2" } @@ -3796,7 +3730,6 @@ "integrity": "sha512-jp2L/eY6fn+KgVVQAOqYItbF0VY/YApe5Mz2F0aykSO8gx31bYCZyvSeYxCHKvzHG5eZjc+zyaS5BrBWya2+kQ==", "devOptional": true, "license": "MIT", - "peer": true, "peerDependencies": { "@types/react": "^19.2.0" } @@ -3884,7 +3817,6 @@ "integrity": "sha512-N9lBGA9o9aqb1hVMc9hzySbhKibHmB+N3IpoShyV6HyQYRGIhlrO5rQgttypi+yEeKsKI4idxC8Jw6gXKD4THA==", "dev": true, "license": "MIT", - "peer": true, "dependencies": { "@typescript-eslint/scope-manager": "8.49.0", "@typescript-eslint/types": "8.49.0", @@ -4239,7 +4171,6 @@ "integrity": "sha512-NZyJarBfL7nWwIq+FDL6Zp/yHEhePMNnnJ0y3qfieCrmNvYct8uvtiV41UvlSe6apAfk0fY1FbWx+NwfmpvtTg==", "dev": true, "license": "MIT", - "peer": true, "bin": { "acorn": "bin/acorn" }, @@ -4303,6 +4234,7 @@ "integrity": "sha512-quJQXlTSUGL2LH9SUXo8VwsY4soanhgo6LNSm84E1LBcE8s3O0wpdiRzyR9z/ZZJMlMWv37qOOb9pdJlMUEKFQ==", "dev": true, "license": "MIT", + "peer": true, "engines": { "node": ">=8" } @@ -4617,7 +4549,6 @@ } ], "license": "MIT", - "peer": true, "dependencies": { "baseline-browser-mapping": "^2.9.0", "caniuse-lite": "^1.0.30001759", @@ -4973,8 +4904,7 @@ "version": "3.2.3", "resolved": "https://registry.npmjs.org/csstype/-/csstype-3.2.3.tgz", "integrity": "sha512-z1HGKcYy2xA8AGQfwrn0PAy+PB7X/GSj3UVJW9qKyn43xWa+gl5nXmU4qqLMRzWVLFC8KusUX8T/0kCiOYpAIQ==", - "license": "MIT", - "peer": true + "license": "MIT" }, "node_modules/data-urls": { "version": "5.0.0", @@ -5237,7 +5167,8 @@ "resolved": "https://registry.npmjs.org/dom-accessibility-api/-/dom-accessibility-api-0.5.16.tgz", "integrity": "sha512-X7BJ2yElsnOJ30pZF4uIIDfBEVgF4XEBxL9Bxhy6dnrm5hkzqmsWHGTiHqRiITNhMyFLyAiWndIJP7Z1NTteDg==", "dev": true, - "license": "MIT" + "license": "MIT", + "peer": true }, "node_modules/dom-helpers": { "version": "5.2.1", @@ -5783,7 +5714,6 @@ "integrity": "sha512-LEyamqS7W5HB3ujJyvi0HQK/dtVINZvd5mAAp9eT5S/ujByGjiZLCzPcHVzuXbpJDJF/cxwHlfceVUDZ2lnSTw==", "dev": true, "license": "MIT", - "peer": true, "dependencies": { "@eslint-community/eslint-utils": "^4.8.0", "@eslint-community/regexpp": "^4.12.1", @@ -5844,7 +5774,6 @@ "integrity": "sha512-82GZUjRS0p/jganf6q1rEO25VSoHH0hKPCTrgillPjdI/3bgBhAE1QzHrHTizjpRvy6pGAvKjDJtk2pF9NDq8w==", "dev": true, "license": "MIT", - "peer": true, "bin": { "eslint-config-prettier": "bin/cli.js" }, @@ -5946,7 +5875,6 @@ "integrity": "sha512-gzUt/qt81nXsFGKIFcC3YnfEAx5NkunCfnDlvuBSSFS02bcXu4Lmea0AFIUwbLWxWPx3d9p8S5QoaujKcNQxcQ==", "dev": true, "license": "MIT", - "peer": true, "funding": { "url": "https://github.com/sponsors/colinhacks" } @@ -6905,7 +6833,6 @@ } ], "license": "MIT", - "peer": true, "dependencies": { "@babel/runtime": "^7.28.4" }, @@ -6939,7 +6866,6 @@ "resolved": "https://registry.npmjs.org/immer/-/immer-10.2.0.tgz", "integrity": "sha512-d/+XTN3zfODyjr89gM3mPq1WNX2B8pYsu7eORitdwyA2sBubnTl3laYlBk4sXY5FUa5qTZGBDPJICVbvqzjlbw==", "license": "MIT", - "peer": true, "funding": { "type": "opencollective", "url": "https://opencollective.com/immer" @@ -7542,7 +7468,6 @@ "integrity": "sha512-Cvc9WUhxSMEo4McES3P7oK3QaXldCfNWp7pl2NNeiIFlCoLr3kfq9kb1fxftiwk1FLV7CvpvDfonxtzUDeSOPg==", "dev": true, "license": "MIT", - "peer": true, "dependencies": { "cssstyle": "^4.2.1", "data-urls": "^5.0.0", @@ -8069,6 +7994,7 @@ "integrity": "sha512-h5bgJWpxJNswbU7qCrV0tIKQCaS3blPDrqKWx+QxzuzL1zGUzij9XCWLrSLsJPu5t+eWA/ycetzYAO5IOMcWAQ==", "dev": true, "license": "MIT", + "peer": true, "bin": { "lz-string": "bin/bin.js" } @@ -9489,7 +9415,6 @@ "integrity": "sha512-v6UNi1+3hSlVvv8fSaoUbggEM5VErKmmpGA7Pl3HF8V6uKY7rvClBOJlH6yNwQtfTueNkGVpOv/mtWL9L4bgRA==", "dev": true, "license": "MIT", - "peer": true, "bin": { "prettier": "bin/prettier.cjs" }, @@ -9606,6 +9531,7 @@ "integrity": "sha512-Qb1gy5OrP5+zDf2Bvnzdl3jsTf1qXVMazbvCoKhtKqVs4/YK4ozX4gKQJJVyNe+cajNPn0KoC0MC3FUmaHWEmQ==", "dev": true, "license": "MIT", + "peer": true, "dependencies": { "ansi-regex": "^5.0.1", "ansi-styles": "^5.0.0", @@ -9621,6 +9547,7 @@ "integrity": "sha512-Cxwpt2SfTzTtXcfOlzGEee8O+c+MmUgGrNiBcXnuWxuFJHe6a5Hz7qwhwe5OgaSYI0IJvkLqWX1ASG+cJOkEiA==", "dev": true, "license": "MIT", + "peer": true, "engines": { "node": ">=10" }, @@ -9710,7 +9637,6 @@ "resolved": "https://registry.npmjs.org/react/-/react-19.2.3.tgz", "integrity": "sha512-Ku/hhYbVjOQnXDZFv2+RibmLFGwFdeeKHFcOTlrt7xplBnya5OGn/hIRDsqDiSUcfORsDC7MPxwork8jBwsIWA==", "license": "MIT", - "peer": true, "engines": { "node": ">=0.10.0" } @@ -9720,7 +9646,6 @@ "resolved": "https://registry.npmjs.org/react-dom/-/react-dom-19.2.3.tgz", "integrity": "sha512-yELu4WmLPw5Mr/lmeEpox5rw3RETacE++JgHqQzd2dg+YbJuat3jH4ingc+WPZhxaoFzdv9y33G+F7Nl5O0GBg==", "license": "MIT", - "peer": true, "dependencies": { "scheduler": "^0.27.0" }, @@ -9733,7 +9658,6 @@ "resolved": "https://registry.npmjs.org/react-hook-form/-/react-hook-form-7.68.0.tgz", "integrity": "sha512-oNN3fjrZ/Xo40SWlHf1yCjlMK417JxoSJVUXQjGdvdRCU07NTFei1i1f8ApUAts+IVh14e4EdakeLEA+BEAs/Q==", "license": "MIT", - "peer": true, "engines": { "node": ">=18.0.0" }, @@ -9777,7 +9701,8 @@ "resolved": "https://registry.npmjs.org/react-is/-/react-is-17.0.2.tgz", "integrity": "sha512-w2GsyukL62IJnlaff/nRegPQR94C/XXamvMWmSHRJ4y7Ts/4ocGRmTHvOs8PSE6pB3dWOrD/nueuU5sduBsQ4w==", "dev": true, - "license": "MIT" + "license": "MIT", + "peer": true }, "node_modules/react-lifecycles-compat": { "version": "3.0.4", @@ -10291,7 +10216,6 @@ "resolved": "https://registry.npmjs.org/seroval/-/seroval-1.5.0.tgz", "integrity": "sha512-OE4cvmJ1uSPrKorFIH9/w/Qwuvi/IMcGbv5RKgcJ/zjA/IohDLU6SVaxFN9FwajbP7nsX0dQqMDes1whk3y+yw==", "license": "MIT", - "peer": true, "engines": { "node": ">=10" } @@ -10833,8 +10757,7 @@ "version": "4.1.18", "resolved": "https://registry.npmjs.org/tailwindcss/-/tailwindcss-4.1.18.tgz", "integrity": "sha512-4+Z+0yiYyEtUVCScyfHCxOYP06L5Ne+JiHhY2IjR2KWMIWhJOYZKLSGZaP5HkZ8+bY0cxfzwDE5uOmzFXyIwxw==", - "license": "MIT", - "peer": true + "license": "MIT" }, "node_modules/tailwindcss-animate": { "version": "1.0.7", @@ -10926,7 +10849,6 @@ "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.3.tgz", "integrity": "sha512-5gTmgEY/sqK6gFXLIsQNH19lWb4ebPDLA4SdLP7dsWkIXHWlG66oPuVvXSGFPppYZz8ZDZq0dYYrbHfBCVUb1Q==", "license": "MIT", - "peer": true, "engines": { "node": ">=12" }, @@ -11170,7 +11092,6 @@ "integrity": "sha512-jl1vZzPDinLr9eUt3J/t7V6FgNEw9QjvBPdysz9KfQDD41fQrC2Y4vKQdiaUpFT4bXlb1RHhLpp8wtm6M5TgSw==", "devOptional": true, "license": "Apache-2.0", - "peer": true, "bin": { "tsc": "bin/tsc", "tsserver": "bin/tsserver" @@ -11507,7 +11428,6 @@ "resolved": "https://registry.npmjs.org/vite/-/vite-6.4.1.tgz", "integrity": "sha512-+Oxm7q9hDoLMyJOYfUYBuHQo+dkAloi33apOPP56pzj+vsdJDzr+j1NISE5pyaAuKL4A3UD34qd0lx5+kfKp2g==", "license": "MIT", - "peer": true, "dependencies": { "esbuild": "^0.25.0", "fdir": "^6.4.4", @@ -11597,8 +11517,7 @@ "resolved": "https://registry.npmjs.org/vite-plugin-electron-renderer/-/vite-plugin-electron-renderer-0.14.6.tgz", "integrity": "sha512-oqkWFa7kQIkvHXG7+Mnl1RTroA4sP0yesKatmAy0gjZC4VwUqlvF9IvOpHd1fpLWsqYX/eZlVxlhULNtaQ78Jw==", "dev": true, - "license": "MIT", - "peer": true + "license": "MIT" }, "node_modules/vite/node_modules/fdir": { "version": "6.5.0", @@ -11636,7 +11555,6 @@ "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.3.tgz", "integrity": "sha512-5gTmgEY/sqK6gFXLIsQNH19lWb4ebPDLA4SdLP7dsWkIXHWlG66oPuVvXSGFPppYZz8ZDZq0dYYrbHfBCVUb1Q==", "license": "MIT", - "peer": true, "engines": { "node": ">=12" }, @@ -12076,7 +11994,6 @@ "resolved": "https://registry.npmjs.org/zod/-/zod-4.3.5.tgz", "integrity": "sha512-k7Nwx6vuWx1IJ9Bjuf4Zt1PEllcwe7cls3VNzm4CQ1/hgtFUK2bRNG3rvnpPUhFjmqJKAKtjV576KnUkHocg/g==", "license": "MIT", - "peer": true, "funding": { "url": "https://github.com/sponsors/colinhacks" } diff --git a/electron/src/machines/properties.ts b/electron/src/machines/properties.ts index a66cd307..8071df00 100644 --- a/electron/src/machines/properties.ts +++ b/electron/src/machines/properties.ts @@ -655,6 +655,41 @@ export const wagoAiTestMachine: MachineProperties = { ], }; +export const testmachinestepper: MachineProperties = { + name: "TestMachineStepper", + version: "V1", + slug: "testmachinestepper", + icon: "lu:Disc3", + machine_identification: { + vendor: VENDOR_QITECH, + machine: 0x0037, + }, + device_roles: [ + { + role: 0, + role_label: "Bus Coupler", + allowed_devices: [ + { + vendor_id: 0x00000021, + product_id: 0x07500354, + revision: 0x2, + }, + ], + }, + { + role: 1, + role_label: "Stepper Motor", + allowed_devices: [ + { + vendor_id: 0x00000021, + product_id: 0x67214e8, + revision: 0x2, + }, + ], + }, + ], +}; + export const machineProperties: MachineProperties[] = [ winder2, extruder3, @@ -672,6 +707,7 @@ export const machineProperties: MachineProperties[] = [ ip20TestMachine, wagoDoTestMachine, TestMotor, + testmachinestepper, ]; export const getMachineProperties = ( diff --git a/electron/src/machines/testmachinestepper/TestMachineStepperControlPage.tsx b/electron/src/machines/testmachinestepper/TestMachineStepperControlPage.tsx new file mode 100644 index 00000000..75526351 --- /dev/null +++ b/electron/src/machines/testmachinestepper/TestMachineStepperControlPage.tsx @@ -0,0 +1,130 @@ +import { ControlCard } from "@/control/ControlCard"; +import { Page } from "@/components/Page"; +import React from "react"; +import { ControlGrid } from "@/control/ControlGrid"; +import { useTestMachineStepper } from "./useTestMachineStepper"; +import { Label } from "@/control/Label"; +import { EditValue } from "@/control/EditValue"; +import { roundToDecimals } from "@/lib/decimal"; +import { TouchButton } from "@/components/touch/TouchButton"; + +export function TestMachineStepperControlPage() { + const { state, setTargetSpeed, setEnabled, setFreq, setAccFreq } = + useTestMachineStepper(); + + return ( + + + + + + + setFreq(0)} + disabled={false} + isLoading={false} + > + Default (00) + + setFreq(1)} + disabled={false} + isLoading={false} + > + Low (01) + + setFreq(2)} + disabled={false} + isLoading={false} + > + Mid (10) + + setFreq(3)} + disabled={false} + isLoading={false} + > + High (11) + + + + setAccFreq(0)} + disabled={false} + isLoading={false} + > + Default (00) + + setAccFreq(1)} + disabled={false} + isLoading={false} + > + Low (01) + + setAccFreq(2)} + disabled={false} + isLoading={false} + > + Mid (10) + + setAccFreq(3)} + disabled={false} + isLoading={false} + > + High (11) + + + + + ); +} diff --git a/electron/src/machines/testmachinestepper/TestMachineStepperPage.tsx b/electron/src/machines/testmachinestepper/TestMachineStepperPage.tsx new file mode 100644 index 00000000..30af8e51 --- /dev/null +++ b/electron/src/machines/testmachinestepper/TestMachineStepperPage.tsx @@ -0,0 +1,20 @@ +import { Topbar } from "@/components/Topbar"; +import { testMachineStepperSerialRoute } from "@/routes/routes"; +import React from "react"; + +export function TestMachineStepperPage() { + const { serial } = testMachineStepperSerialRoute.useParams(); + return ( + + ); +} diff --git a/electron/src/machines/testmachinestepper/testMachineStepperNamespace.ts b/electron/src/machines/testmachinestepper/testMachineStepperNamespace.ts new file mode 100644 index 00000000..d56f1adf --- /dev/null +++ b/electron/src/machines/testmachinestepper/testMachineStepperNamespace.ts @@ -0,0 +1,81 @@ +import { StoreApi } from "zustand"; +import { create } from "zustand"; +import { z } from "zod"; +import { + EventHandler, + eventSchema, + Event, + handleUnhandledEventError, + NamespaceId, + createNamespaceHookImplementation, + ThrottledStoreUpdater, +} from "@/client/socketioStore"; +import { MachineIdentificationUnique } from "@/machines/types"; + +// ========== Event Schema ========== + +export const stateEventDataSchema = z.object({ + target_speed: z.number(), + enabled: z.boolean(), + freq: z.number(), + acc_freq: z.number(), +}); + +export const stateEventSchema = eventSchema(stateEventDataSchema); + +export type StateEvent = z.infer; + +// ========== Store ========== +export type TestMachineStepperNamespaceStore = { + state: StateEvent | null; +}; + +export const createTestMachineStepperNamespaceStore = + (): StoreApi => + create(() => ({ + state: null, + })); + +// ========== Message Handler ========== +export function testMachineStepperMessageHandler( + store: StoreApi, + throttledUpdater: ThrottledStoreUpdater, +): EventHandler { + return (event: Event) => { + const updateStore = ( + updater: ( + state: TestMachineStepperNamespaceStore, + ) => TestMachineStepperNamespaceStore, + ) => throttledUpdater.updateWith(updater); + + try { + if (event.name === "StateEvent") { + const parsed = stateEventSchema.parse(event); + updateStore(() => ({ state: parsed.data })); + } else { + handleUnhandledEventError(event.name); + } + } catch (error) { + console.error(`Error processing ${event.name}:`, error); + throw error; + } + }; +} + +// ========== Namespace Hook ========== +const useTestMachineStepperNamespaceImplementation = + createNamespaceHookImplementation({ + createStore: createTestMachineStepperNamespaceStore, + createEventHandler: testMachineStepperMessageHandler, + }); + +export function useTestMachineStepperNamespace( + machine_identification_unique: MachineIdentificationUnique, +): TestMachineStepperNamespaceStore { + const namespaceId: NamespaceId = { + type: "machine", + machine_identification_unique, + }; + + return useTestMachineStepperNamespaceImplementation(namespaceId); +} diff --git a/electron/src/machines/testmachinestepper/useTestMachineStepper.ts b/electron/src/machines/testmachinestepper/useTestMachineStepper.ts new file mode 100644 index 00000000..0083cf73 --- /dev/null +++ b/electron/src/machines/testmachinestepper/useTestMachineStepper.ts @@ -0,0 +1,126 @@ +import { toastError } from "@/components/Toast"; +import { useStateOptimistic } from "@/lib/useStateOptimistic"; +import { testMachineStepperSerialRoute } from "@/routes/routes"; +import { MachineIdentificationUnique } from "@/machines/types"; +import { + useTestMachineStepperNamespace, + StateEvent, +} from "./testMachineStepperNamespace"; +import { useMachineMutate } from "@/client/useClient"; +import { produce } from "immer"; +import { useEffect, useMemo } from "react"; +import { testmachinestepper } from "@/machines/properties"; +import { z } from "zod"; +export function useTestMachineStepper() { + const { serial: serialString } = testMachineStepperSerialRoute.useParams(); + + // Memoize machine identification + const machineIdentification: MachineIdentificationUnique = useMemo(() => { + const serial = parseInt(serialString); + + if (isNaN(serial)) { + toastError( + "Invalid Serial Number", + `"${serialString}" is not a valid serial number.`, + ); + + return { + machine_identification: { vendor: 0, machine: 0 }, + serial: 0, + }; + } + + return { + machine_identification: testmachinestepper.machine_identification, + serial, + }; + }, [serialString]); + + // Namespace state from backend + const { state } = useTestMachineStepperNamespace(machineIdentification); + + // Optimistic state + const stateOptimistic = useStateOptimistic(); + + useEffect(() => { + if (state) stateOptimistic.setReal(state); + }, [state, stateOptimistic]); + + // Generic mutation sender + const { request: sendMutation } = useMachineMutate( + z.object({ + action: z.string(), + value: z.any(), + }), + ); + + const updateStateOptimistically = ( + producer: (current: StateEvent) => void, + serverRequest?: () => void, + ) => { + const currentState = stateOptimistic.value; + if (currentState) + stateOptimistic.setOptimistic(produce(currentState, producer)); + serverRequest?.(); + }; + + const setTargetSpeed = (target: number) => { + updateStateOptimistically( + (current) => { + current.target_speed = target; + }, + () => + sendMutation({ + machine_identification_unique: machineIdentification, + data: { action: "SetTargetSpeed", value: { target } }, + }), + ); + }; + + const setEnabled = (enabled: boolean) => { + updateStateOptimistically( + (current) => { + current.enabled = enabled; + }, + () => + sendMutation({ + machine_identification_unique: machineIdentification, + data: { action: "SetEnabled", value: { enabled } }, + }), + ); + }; + + const setFreq = (factor: number) => { + updateStateOptimistically( + (current) => { + current.freq = factor; + }, + () => + sendMutation({ + machine_identification_unique: machineIdentification, + data: { action: "SetFreq", value: { factor } }, + }), + ); + }; + + const setAccFreq = (factor: number) => { + updateStateOptimistically( + (current) => { + current.acc_freq = factor; + }, + () => + sendMutation({ + machine_identification_unique: machineIdentification, + data: { action: "SetAccFreq", value: { factor } }, + }), + ); + }; + + return { + state: stateOptimistic.value, + setTargetSpeed, + setEnabled, + setFreq, + setAccFreq, + }; +} diff --git a/electron/src/routes/routes.tsx b/electron/src/routes/routes.tsx index 931cfa07..7d9895d3 100644 --- a/electron/src/routes/routes.tsx +++ b/electron/src/routes/routes.tsx @@ -57,6 +57,8 @@ import { Aquapath1SettingsPage } from "@/machines/aquapath/aquapath1/Aquapath1Se import { TestMachinePage } from "@/machines/testmachine/TestMachinePage"; import { TestMachineControlPage } from "@/machines/testmachine/TestMachineControlPage"; +import { TestMachineStepperPage } from "@/machines/testmachinestepper/TestMachineStepperPage"; +import { TestMachineStepperControlPage } from "@/machines/testmachinestepper/TestMachineStepperControlPage"; import { AnalogInputTestMachine } from "@/machines/analoginputtestmachine/AnalogInputTestMachinePage"; import { AnalogInputTestMachineControl } from "@/machines/analoginputtestmachine/AnalogInputTestMachineControlPage"; import { WagoAiTestMachine } from "@/machines/wagoaitestmachine/WagoAiTestMachinePage"; @@ -172,6 +174,19 @@ export const testMotorControlRoute = createRoute({ component: () => , }); +export const testMachineStepperSerialRoute = createRoute({ + getParentRoute: () => machinesRoute, + path: "testmachinestepper/$serial", + component: () => , +}); + +export const testMachineStepperControlRoute = createRoute({ + getParentRoute: () => testMachineStepperSerialRoute, + path: "control", + component: () => , +}); + +// Leaf route: control page export const sidebarRoute = createRoute({ getParentRoute: () => RootRoute, path: "_sidebar", @@ -515,6 +530,10 @@ export const rootTree = RootRoute.addChildren([ testMachineSerialRoute.addChildren([testMachineControlRoute]), + testMachineStepperSerialRoute.addChildren([ + testMachineStepperControlRoute, + ]), + analogInputTestMachineSerialRoute.addChildren([ analogInputTestMachineControlRoute, ]), diff --git a/ethercat-hal/src/devices/wago_750_354.rs b/ethercat-hal/src/devices/wago_750_354.rs index 3f792dea..8e508426 100755 --- a/ethercat-hal/src/devices/wago_750_354.rs +++ b/ethercat-hal/src/devices/wago_750_354.rs @@ -15,6 +15,7 @@ use crate::{ wago_750_501::{WAGO_750_501_MODULE_IDENT, WAGO_750_501_PRODUCT_ID}, wago_750_530::{WAGO_750_530_MODULE_IDENT, WAGO_750_530_PRODUCT_ID}, wago_750_652::{WAGO_750_652_MODULE_IDENT, WAGO_750_652_PRODUCT_ID}, + wago_750_672::{WAGO_750_672_MODULE_IDENT, WAGO_750_672_PRODUCT_ID}, wago_750_1506::{WAGO_750_1506_MODULE_IDENT, WAGO_750_1506_PRODUCT_ID}, }, }, @@ -62,7 +63,6 @@ impl EthercatDevice for Wago750_354 { None => break, } } - Ok(()) } @@ -153,14 +153,22 @@ impl std::fmt::Debug for Wago750_354 { impl Wago750_354 { pub fn calculate_module_index(pdo_mapping: u32, is_tx: bool) -> u32 { let start_index = match is_tx { - true => 0x6000 as u32, - false => 0x7000 as u32, + true => 0x6000, + false => 0x7000, }; - let index_in_hex = ((pdo_mapping & 0xFFFF0000) >> 16) - start_index; - if index_in_hex < 16 { + let pdo_index = (pdo_mapping & 0xFFFF0000) >> 16; + + if pdo_index < start_index { + // Treat as coupler/module 0 return 0; + } + + let index_in_hex = pdo_index - start_index; + + if index_in_hex < 16 { + 0 } else { - return index_in_hex / 16; + index_in_hex / 16 } } @@ -191,7 +199,7 @@ impl Wago750_354 { } let mut mappings_without_coupler: Vec = vec![]; - for i in start_subindex..count_mappings { + for i in start_subindex..=count_mappings { let pdo_index = device.sdo_read(index.0, i).await?; let pdo_map_count = device.sdo_read::(pdo_index, 0).await?; for j in 0..pdo_map_count { @@ -303,6 +311,11 @@ impl Wago750_354 { module.has_rx = false; module.name = "750-430".to_string(); } + WAGO_750_672_PRODUCT_ID => { + module.has_tx = true; + module.has_rx = true; + module.name = "750-672".to_string(); + } _ => println!( "Wago-750-354 found Unknown/Unimplemented Module: {}", ident_iom @@ -383,7 +396,9 @@ impl Wago750_354 { WAGO_750_430_MODULE_IDENT => { Arc::new(RwLock::new(wago_750_430::Wago750_430::new())) } - + WAGO_750_672_MODULE_IDENT => { + Arc::new(RwLock::new(wago_750_672::Wago750_672::new())) + } _ => { println!( "{} Missing Implementation for Module Identification: vendor_id: {:?}, module ident: {:?} !", diff --git a/ethercat-hal/src/devices/wago_modules/mod.rs b/ethercat-hal/src/devices/wago_modules/mod.rs index 21e48dc7..3c079875 100644 --- a/ethercat-hal/src/devices/wago_modules/mod.rs +++ b/ethercat-hal/src/devices/wago_modules/mod.rs @@ -6,3 +6,4 @@ pub mod wago_750_455; pub mod wago_750_501; pub mod wago_750_530; pub mod wago_750_652; +pub mod wago_750_672; diff --git a/ethercat-hal/src/devices/wago_modules/wago_750_1506.rs b/ethercat-hal/src/devices/wago_modules/wago_750_1506.rs index fd700fd2..90777b77 100644 --- a/ethercat-hal/src/devices/wago_modules/wago_750_1506.rs +++ b/ethercat-hal/src/devices/wago_modules/wago_750_1506.rs @@ -143,7 +143,7 @@ pub struct Wago750_1506 { is_used: bool, tx_bit_offset: usize, rx_bit_offset: usize, - // Should always have on but not when calling constructor + // Should always have one but not when calling constructor module: Option, rx_pdo: Wago750_1506RxPdo, tx_pdo: Wago750_1506TxPdo, diff --git a/ethercat-hal/src/devices/wago_modules/wago_750_672.rs b/ethercat-hal/src/devices/wago_modules/wago_750_672.rs new file mode 100644 index 00000000..bcd3c6e0 --- /dev/null +++ b/ethercat-hal/src/devices/wago_modules/wago_750_672.rs @@ -0,0 +1,387 @@ +/* + * Wago Stepper Controller 750-672 + * 70 VDC / 7.5 A + */ + +use anyhow::Ok; +use bitvec::field::BitField; + +use crate::{ + devices::{ + DynamicEthercatDevice, EthercatDevice, EthercatDeviceProcessing, EthercatDeviceUsed, + EthercatDynamicPDO, Module, NewEthercatDevice, SubDeviceProductTuple, + }, + io::{ + digital_input::{DigitalInputDevice, DigitalInputInput}, + stepper_velocity_wago_750_672::{ + C1Command, C1Flag, C2Flag, C3Flag, ControlByteC1, ControlByteC2, ControlByteC3, S3Flag, + StatusByteS3, + }, + }, +}; + +#[derive(Clone)] +pub struct Wago750_672 { + is_used: bool, + tx_bit_offset: usize, + rx_bit_offset: usize, + pub rxpdo: Wago750_672RxPdo, + pub txpdo: Wago750_672TxPdo, + module: Option, + pub state: InitState, + pub initialized: bool, +} + +// unfortunately we need to track and set bits over multiple cycles +// and wait for their acknowledgement so we have to use a state machine +// inside of our loop. +#[derive(Debug, Clone)] +pub enum InitState { + Off, + Enable, + SetMode, + StartPulseStart, + StartPulseEnd, + Running, + ErrorQuit, + ResetQuit, +} + +/* +* Wago has two different kind of applications when it comes to +* running the Stepper Controller. +* - Positioning +* - Frequency/Speed Control +* !!!! IMPORTANT !!!! +* It seems like this is only true for the 750 671. +* The 750 672 has differnt commands inside of ControlByteC1 +* where w can choose the Speed control mode - just like that +* we always want to use the latter one so the following +* process images are for that usecase. +*/ + +#[derive(Clone, Debug, Default)] +pub struct Wago750_672RxPdo { + pub control_byte: u8, // C0 + pub velocity: i16, // D0/D1 + pub acceleration: u16, // D2/D3 + pub control_byte3: u8, // C3 + pub control_byte2: u8, // C2 + pub control_byte1: u8, // C1 +} + +#[derive(Clone, Debug, Default)] +pub struct Wago750_672TxPdo { + pub status_byte0: u8, // S0 + pub actual_velocity: i16, // D0/D1 + pub position_l: u8, // D4 + pub position_m: u8, // D5 + pub position_h: u8, // D6 + pub status_byte3: u8, // S3 + pub status_byte2: u8, // S2 + pub status_byte1: u8, // S1 +} + +/// Digital input port enumeration for the 6 inputs +#[derive(Debug, Clone, Copy)] +pub enum Wago750_672InputPort { + DI1, + DI2, + DI3, + DI4, + DI5, + DI6, +} + +// Get the Digital Inputs from the Status Byte S3 +impl DigitalInputDevice for Wago750_672 { + fn get_input(&self, port: Wago750_672InputPort) -> Result { + let s3 = StatusByteS3::from_bits(self.txpdo.status_byte3); + Ok(DigitalInputInput { + value: match port { + Wago750_672InputPort::DI1 => s3.has_flag(S3Flag::Input1), + Wago750_672InputPort::DI2 => s3.has_flag(S3Flag::Input2), + Wago750_672InputPort::DI3 => s3.has_flag(S3Flag::Input3), + Wago750_672InputPort::DI4 => s3.has_flag(S3Flag::Input4), + Wago750_672InputPort::DI5 => s3.has_flag(S3Flag::Input5), + Wago750_672InputPort::DI6 => s3.has_flag(S3Flag::Input6), + }, + }) + } +} + +impl EthercatDeviceUsed for Wago750_672 { + fn is_used(&self) -> bool { + self.is_used + } + fn set_used(&mut self, used: bool) { + self.is_used = used; + } +} + +impl DynamicEthercatDevice for Wago750_672 {} + +impl EthercatDynamicPDO for Wago750_672 { + fn get_tx_offset(&self) -> usize { + self.tx_bit_offset + } + fn get_rx_offset(&self) -> usize { + self.rx_bit_offset + } + fn set_tx_offset(&mut self, offset: usize) { + self.tx_bit_offset = offset + } + fn set_rx_offset(&mut self, offset: usize) { + self.rx_bit_offset = offset + } +} + +impl EthercatDeviceProcessing for Wago750_672 { + fn input_post_process(&mut self) -> Result<(), anyhow::Error> { + Ok(()) + } + + fn output_pre_process(&mut self) -> Result<(), anyhow::Error> { + Ok(()) + } +} + +impl EthercatDevice for Wago750_672 { + fn input( + &mut self, + input: &bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + let base = self.tx_bit_offset; + + let mut b = [0u8; 12]; + for i in 0..12 { + b[i] = input[base + i * 8..base + (i + 1) * 8].load_le(); + } + + self.txpdo = Wago750_672TxPdo { + status_byte0: b[0], + actual_velocity: i16::from_le_bytes([b[2], b[3]]), + position_l: b[6], + position_m: b[7], + position_h: b[8], + status_byte3: b[9], + status_byte2: b[10], + status_byte1: b[11], + }; + + // Yes this statemachine is unfortunately needed in here to make sure + // bits are correctly set and reset in the correct cycle. + match self.state { + InitState::Off => { + // Do nothing + self.initialized = false; + } + InitState::Enable => { + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .bits(); + self.rxpdo.control_byte1 = c1; + + // Switch state if ENABLE and STOP2_N is acknowledged + if self.txpdo.status_byte1 == c1 { + self.state = InitState::SetMode; + } + } + InitState::SetMode => { + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.control_byte1 = c1; + + // Switch state if SPEED MODE is acknowledged + if self.txpdo.status_byte1 == c1 { + self.initialized = true; + self.state = InitState::StartPulseStart; + } + } + InitState::StartPulseStart => { + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_flag(C1Flag::Start) + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.control_byte1 = c1; + + // Switch state after StartPulse is acknowledged + if self.txpdo.status_byte1 == c1 { + self.state = InitState::StartPulseEnd; + } + } + InitState::StartPulseEnd => { + let c1 = ControlByteC1::new() + .with_flag(C1Flag::Enable) + .with_flag(C1Flag::Stop2N) + .with_command(C1Command::SpeedControl) + .bits(); + self.rxpdo.control_byte1 = c1; + + // Switch state after StartPulse is over + if self.txpdo.status_byte1 == c1 { + self.state = InitState::Running; + } + } + InitState::Running => { + let c2 = ControlByteC2::new().with_flag(C2Flag::ErrorQuit).bits(); + let c3 = ControlByteC3::new().with_flag(C3Flag::ResetQuit).bits(); + + // Check for Error + if self.txpdo.status_byte2 & c2 != 0 { + self.state = InitState::ErrorQuit; + } else { + self.rxpdo.control_byte2 |= c2; + } + // Check for Reset + if self.txpdo.status_byte3 & c3 != 0 { + self.state = InitState::ResetQuit; + } else { + self.rxpdo.control_byte3 |= c3; + } + } + InitState::ErrorQuit => { + self.rxpdo.control_byte2 |= + ControlByteC2::new().with_flag(C2Flag::ErrorQuit).bits(); + tracing::error!("Stepper Controller Errored. Trying to reenable..."); + self.state = InitState::Enable; + } + InitState::ResetQuit => { + self.rxpdo.control_byte3 |= + ControlByteC3::new().with_flag(C3Flag::ResetQuit).bits(); + tracing::error!("Stepper Controller Reset not Quit. Trying to reenable..."); + self.state = InitState::Enable; + } + } + + Ok(()) + } + + fn input_len(&self) -> usize { + 12 * 8 + } + + fn output( + &self, + output: &mut bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + let base = self.rx_bit_offset; + + let output_bytes = [ + self.rxpdo.control_byte, + 0, + self.rxpdo.velocity.to_le_bytes()[0], + self.rxpdo.velocity.to_le_bytes()[1], + self.rxpdo.acceleration.to_le_bytes()[0], + self.rxpdo.acceleration.to_le_bytes()[1], + 0, + 0, + 0, + self.rxpdo.control_byte3, + self.rxpdo.control_byte2, + self.rxpdo.control_byte1, + ]; + + for i in 0..12 { + output[base + i * 8..base + (i + 1) * 8].store_le(output_bytes[i]); + } + + Ok(()) + } + + fn output_len(&self) -> usize { + 12 * 8 + } + + fn as_any(&self) -> &dyn std::any::Any { + self + } + + fn as_any_mut(&mut self) -> &mut dyn std::any::Any { + self + } + + fn is_module(&self) -> bool { + true + } + + fn input_checked( + &mut self, + input: &bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + let expected = self.input_len(); + let actual = input.len(); + if actual != expected { + return Err(anyhow::anyhow!( + "[{}::Device::input_checked] Input length is {} ({} bytes) and must be {} bits ({} bytes)", + module_path!(), + actual, + actual / 8, + expected, + expected / 8 + )); + } + Ok(()) + } + + fn output_checked( + &self, + output: &mut bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + let expected = self.output_len(); + let actual = output.len(); + if actual != expected { + return Err(anyhow::anyhow!( + "[{}::Device::output_checked] Output length is {} ({} bytes) and must be {} bits ({} bytes)", + module_path!(), + actual, + actual / 8, + expected, + expected / 8 + )); + } + Ok(()) + } + + fn get_module(&self) -> Option { + self.module.clone() + } + + fn set_module(&mut self, module: crate::devices::Module) { + self.tx_bit_offset = module.tx_offset; + self.rx_bit_offset = module.rx_offset; + self.module = Some(module) + } +} + +impl NewEthercatDevice for Wago750_672 { + fn new() -> Self { + Self { + is_used: false, + tx_bit_offset: 0, + rx_bit_offset: 0, + module: None, + rxpdo: Wago750_672RxPdo::default(), + txpdo: Wago750_672TxPdo::default(), + state: InitState::Off, + initialized: false, + } + } +} + +impl std::fmt::Debug for Wago750_672 { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "Wago750_672") + } +} + +pub const WAGO_750_672_VENDOR_ID: u32 = 0x00000021; +pub const WAGO_750_672_PRODUCT_ID: u32 = 108139752; +pub const WAGO_750_672_MODULE_IDENT: SubDeviceProductTuple = + (WAGO_750_672_VENDOR_ID, WAGO_750_672_PRODUCT_ID); diff --git a/ethercat-hal/src/helpers/counter_wrapper_u16_i128.rs b/ethercat-hal/src/helpers/counter_wrapper_u16_i128.rs index cf6b3579..d714ef75 100644 --- a/ethercat-hal/src/helpers/counter_wrapper_u16_i128.rs +++ b/ethercat-hal/src/helpers/counter_wrapper_u16_i128.rs @@ -5,7 +5,7 @@ const U16_MAX: i128 = std::u16::MAX as i128; /// We convert the overflows and underflows to an i128 value for easier calculations. /// /// When overriding the counter we don't set the valeu direclty but schedula it wiht the [`push_override`] so it can be synced with [`pop_override`] to an EtherCAT device. -#[derive(Debug)] +#[derive(Clone, Debug)] pub struct CounterWrapperU16U128 { counter: i128, last_counter: u16, diff --git a/ethercat-hal/src/io/mod.rs b/ethercat-hal/src/io/mod.rs index 4fcf85ed..d603a7cc 100644 --- a/ethercat-hal/src/io/mod.rs +++ b/ethercat-hal/src/io/mod.rs @@ -7,4 +7,5 @@ pub mod encoder_input; pub mod pulse_train_output; pub mod serial_interface; pub mod stepper_velocity_el70x1; +pub mod stepper_velocity_wago_750_672; pub mod temperature_input; diff --git a/ethercat-hal/src/io/stepper_velocity_wago_750_672.rs b/ethercat-hal/src/io/stepper_velocity_wago_750_672.rs new file mode 100644 index 00000000..6c67bfea --- /dev/null +++ b/ethercat-hal/src/io/stepper_velocity_wago_750_672.rs @@ -0,0 +1,376 @@ +use std::sync::Arc; + +use smol::{block_on, lock::RwLock}; + +use crate::devices::wago_modules::wago_750_672::{InitState, Wago750_672}; + +/* + * Wago Stepper Velocity Wrapper around Stepper Controller + * + */ +#[derive(Debug)] +pub struct StepperVelocityWago750672 { + pub device: Arc>, + pub state: InitState, + pub target_velocity: i16, + pub target_acceleration: u16, + pub enabled: bool, + pub freq_range_sel: u8, + pub acc_range_sel: u8, +} + +impl StepperVelocityWago750672 { + pub fn new(device: Arc>) -> Self { + Self { + device, + state: InitState::Off, + target_velocity: 0, + target_acceleration: 10000, + enabled: false, + freq_range_sel: 0, + acc_range_sel: 0, + } + } + + pub fn set_enabled(&mut self, enabled: bool) { + if self.enabled && enabled { + return; + } + + // this needs to be set before the stepper controller + // is enabled because it will generate an error otherwise + self.set_acceleration(self.target_acceleration); + + self.enabled = enabled; + if enabled { + self.change_init_state(InitState::Enable); + } else { + self.change_init_state(InitState::Off); + self.write_control_byte(ControlByte::C1, 0b00000000); + } + } + + pub fn set_velocity(&mut self, velocity: i16) { + self.target_velocity = velocity; + + let mut dev = block_on(self.device.write()); + + // clamp velocity to -25000 - 25000 :: because this is + // the min max for the controller + dev.rxpdo.velocity = velocity.clamp(-25000, 25000); + + if dev.initialized { + dev.state = InitState::StartPulseStart; + } + } + + pub fn set_acceleration(&mut self, acceleration: u16) { + self.target_acceleration = acceleration; + + let mut dev = block_on(self.device.write()); + + dev.rxpdo.acceleration = acceleration; + } + + #[allow(dead_code)] + fn get_actual_velocity(&self) -> i16 { + let dev = block_on(self.device.read()); + dev.txpdo.actual_velocity + } + + #[allow(dead_code)] + fn get_target_acceleration(&self) -> u16 { + let dev = block_on(self.device.read()); + dev.rxpdo.acceleration + } + + pub fn set_freq_range_sel(&mut self, factor: u8) { + if self.enabled || factor > 3 { + return; + } + self.freq_range_sel = factor; + let mut dev = block_on(self.device.write()); + let c2 = ControlByteC2::from_bits(dev.rxpdo.control_byte2) + .with_freq_range(factor) + .bits(); + dev.rxpdo.control_byte2 = c2; + } + + pub fn set_acc_range_sel(&mut self, factor: u8) { + if self.enabled || factor > 3 { + return; + } + self.acc_range_sel = factor; + let mut dev = block_on(self.device.write()); + let c2 = ControlByteC2::from_bits(dev.rxpdo.control_byte2) + .with_acc_range(factor) + .bits(); + dev.rxpdo.control_byte2 = c2; + } + + fn change_init_state(&mut self, state: InitState) { + self.state = state.clone(); + let mut dev = block_on(self.device.write()); + dev.state = state; + } + + fn write_control_byte(&self, control_byte: ControlByte, value: u8) { + let mut dev = block_on(self.device.write()); + + match control_byte { + ControlByte::C0 => dev.rxpdo.control_byte = value, + ControlByte::C1 => dev.rxpdo.control_byte1 = value, + ControlByte::C2 => dev.rxpdo.control_byte2 = value, + ControlByte::C3 => dev.rxpdo.control_byte3 = value, + } + } + + #[allow(dead_code)] + fn read_status_byte(&self, status_byte: StatusByte) -> u8 { + let dev = block_on(self.device.read()); + + match status_byte { + StatusByte::S0 => dev.txpdo.status_byte0, + StatusByte::S1 => dev.txpdo.status_byte1, + StatusByte::S2 => dev.txpdo.status_byte2, + StatusByte::S3 => dev.txpdo.status_byte3, + } + } +} + +// The Different Control Bytes set control the stepper controller +// by setting and resetting specific bits. +// +// There are some construction functions provided to create the +// control bytes correctly. + +/// Control Byte C1 +#[derive(Clone, Copy, Default)] +pub struct ControlByteC1(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum C1Flag { + Enable = 0b0000_0001, + Stop2N = 0b0000_0010, + Start = 0b0000_0100, +} + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum C1Command { + Idle = 0b0000_0000, + SinglePosition = 0b0000_1000, + RunProgram = 0b0001_0000, + SpeedControl = 0b0001_1000, + Reference = 0b0010_0000, + JogMode = 0b0010_1000, + Mailbox = 0b0011_0000, +} + +impl ControlByteC1 { + pub const fn new() -> Self { + Self(0) + } + + pub const fn with_flag(mut self, flag: C1Flag) -> Self { + self.0 |= flag as u8; + self + } + + pub const fn with_command(mut self, cmd: C1Command) -> Self { + self.0 = (self.0 & 0b0000_0111) | (cmd as u8); + self + } + + pub const fn bits(self) -> u8 { + self.0 + } +} + +/// Control Byte C2 +#[derive(Clone, Copy, Default)] +pub struct ControlByteC2(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum C2Flag { + FreqRangeSelL = 0b0000_0001, + FreqRangeSelH = 0b0000_0010, + AccRangeSelL = 0b0000_0100, + AccRangeSelH = 0b0000_1000, + PreCalc = 0b0100_0000, + ErrorQuit = 0b1000_0000, +} + +impl ControlByteC2 { + pub const fn new() -> Self { + Self(0) + } + + pub const fn from_bits(bits: u8) -> Self { + Self(bits) + } + + pub const fn with_freq_range(mut self, sel: u8) -> Self { + self.0 = (self.0 & 0b1111_1100) | (sel & 0b0000_0011); + self + } + + pub const fn with_acc_range(mut self, sel: u8) -> Self { + self.0 = (self.0 & 0b1111_0011) | ((sel & 0b0000_0011) << 2); + self + } + + pub const fn with_flag(mut self, flag: C2Flag) -> Self { + self.0 |= flag as u8; + self + } + + pub const fn bits(self) -> u8 { + self.0 + } +} + +/// Control Byte C3 +#[derive(Clone, Copy, Default)] +pub struct ControlByteC3(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum C3Flag { + SetActualPos = 0b0000_0001, + DirectionPos = 0b0000_0010, + DirectionNeg = 0b0000_0100, + ResetQuit = 0b1000_0000, +} + +impl ControlByteC3 { + pub const fn new() -> Self { + Self(0) + } + + pub const fn with_flag(mut self, flag: C3Flag) -> Self { + self.0 |= flag as u8; + self + } + + pub const fn bits(self) -> u8 { + self.0 + } +} + +// The status bytes are similar to the control bytes but are only readable +// and most of the time provide corresponding acknoledgements to the diffrent +// Control bytes. +// +// There are also helper functions to construct the diffrent Status bytes for +// compoarison. + +/// Status Byte S1 +#[derive(Clone, Copy, Default)] +pub struct StatusByteS1(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum S1Flag { + Ready = 0b0000_0001, + Stop2NAck = 0b0000_0010, + StartAck = 0b0000_0100, +} + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum S1CommandAck { + Idle = 0b0000_0000, + SinglePosition = 0b0000_1000, + RunProgram = 0b0001_0000, + SpeedControl = 0b0001_1000, + Reference = 0b0010_0000, + JogMode = 0b0010_1000, + Mailbox = 0b0011_0000, +} + +impl StatusByteS1 { + pub const fn from_bits(bits: u8) -> Self { + Self(bits) + } + + pub const fn has_flag(self, flag: S1Flag) -> bool { + (self.0 & flag as u8) != 0 + } + + pub const fn bits(self) -> u8 { + self.0 + } +} + +/// Status Byte S2 +#[derive(Clone, Copy, Default)] +pub struct StatusByteS2(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum S2Flag { + OnTarget = 0b0000_0001, + Busy = 0b0000_0010, + StandStill = 0b0000_0100, + OnSpeed = 0b0000_1000, + Direction = 0b0001_0000, + ReferenceOk = 0b0010_0000, + PreCalcAck = 0b0100_0000, + Error = 0b1000_0000, +} + +impl StatusByteS2 { + pub const fn from_bits(bits: u8) -> Self { + Self(bits) + } + + pub const fn has_flag(self, flag: S2Flag) -> bool { + (self.0 & flag as u8) != 0 + } +} + +/// Status Byte S3 +#[derive(Clone, Copy, Default)] +pub struct StatusByteS3(u8); + +#[repr(u8)] +#[derive(Clone, Copy)] +pub enum S3Flag { + Input1 = 0b0000_0001, + Input2 = 0b0000_0010, + Input3 = 0b0000_0100, + Input4 = 0b0000_1000, + Input5 = 0b0001_0000, + Input6 = 0b0010_0000, + Warning = 0b0100_0000, + Reset = 0b1000_0000, +} + +impl StatusByteS3 { + pub const fn from_bits(bits: u8) -> Self { + Self(bits) + } + + pub const fn has_flag(self, flag: S3Flag) -> bool { + (self.0 & flag as u8) != 0 + } +} + +#[derive(Debug, Clone, Copy)] +pub enum ControlByte { + C0, + C1, + C2, + C3, +} + +#[derive(Debug, Clone, Copy)] +pub enum StatusByte { + S0, + S1, + S2, + S3, +} diff --git a/machines/src/lib.rs b/machines/src/lib.rs index 8952b6c8..297d235a 100644 --- a/machines/src/lib.rs +++ b/machines/src/lib.rs @@ -32,6 +32,7 @@ pub mod motor_test_machine; pub mod registry; pub mod serial; pub mod test_machine; +pub mod test_machine_stepper; pub mod wago_8ch_dio_test_machine; pub mod wago_ai_test_machine; pub mod wago_do_test_machine; @@ -53,6 +54,7 @@ pub const ANALOG_INPUT_TEST_MACHINE: u16 = 0x0035; pub const WAGO_AI_TEST_MACHINE: u16 = 0x0036; pub const DIGITAL_INPUT_TEST_MACHINE: u16 = 0x0040; pub const WAGO_8CH_IO_TEST_MACHINE: u16 = 0x0041; +pub const TEST_MACHINE_STEPPER: u16 = 0x0037; pub const MOTOR_TEST_MACHINE: u16 = 0x0011; pub const WAGO_DO_TEST_MACHINE: u16 = 0x000E; diff --git a/machines/src/registry.rs b/machines/src/registry.rs index 478b7124..846788c2 100644 --- a/machines/src/registry.rs +++ b/machines/src/registry.rs @@ -22,7 +22,7 @@ use crate::{ winder2::Winder2, }; -use crate::test_machine::TestMachine; +use crate::{test_machine::TestMachine, test_machine_stepper::TestMachineStepper}; use lazy_static::lazy_static; @@ -144,6 +144,7 @@ lazy_static! { Wago8chDigitalIOTestMachine::MACHINE_IDENTIFICATION, ); + mc.register::(TestMachineStepper::MACHINE_IDENTIFICATION); mc }; } diff --git a/machines/src/test_machine_stepper/act.rs b/machines/src/test_machine_stepper/act.rs new file mode 100644 index 00000000..73a062eb --- /dev/null +++ b/machines/src/test_machine_stepper/act.rs @@ -0,0 +1,33 @@ +use super::TestMachineStepper; +use crate::{MachineAct, MachineMessage}; +use std::time::{Duration, Instant}; + +impl MachineAct for TestMachineStepper { + fn act(&mut self, now: Instant) { + if let Ok(msg) = self.api_receiver.try_recv() { + self.act_machine_message(msg); + } + + if now.duration_since(self.last_state_emit) > Duration::from_secs_f64(1.0 / 30.0) { + self.emit_state(); + self.last_state_emit = now; + } + } + + fn act_machine_message(&mut self, msg: MachineMessage) { + match msg { + MachineMessage::SubscribeNamespace(namespace) => { + self.namespace.namespace = Some(namespace); + self.emit_state(); + } + MachineMessage::UnsubscribeNamespace => self.namespace.namespace = None, + MachineMessage::HttpApiJsonRequest(value) => { + use crate::MachineApi; + let _res = self.api_mutate(value); + } + MachineMessage::ConnectToMachine(_machine_connection) => {} + MachineMessage::DisconnectMachine(_machine_connection) => {} + MachineMessage::RequestValues(_sender) => {} + } + } +} diff --git a/machines/src/test_machine_stepper/api.rs b/machines/src/test_machine_stepper/api.rs new file mode 100644 index 00000000..9736c322 --- /dev/null +++ b/machines/src/test_machine_stepper/api.rs @@ -0,0 +1,87 @@ +use super::TestMachineStepper; +use crate::{MachineApi, MachineMessage}; +use control_core::socketio::{ + event::{Event, GenericEvent}, + namespace::{ + CacheFn, CacheableEvents, Namespace, NamespaceCacheingLogic, cache_first_and_last_event, + }, +}; +use serde::{Deserialize, Serialize}; +use serde_json::Value; +use std::sync::Arc; + +#[derive(Serialize, Debug, Clone)] +pub struct StateEvent { + pub target_speed: i16, + pub enabled: bool, + pub freq: u8, + pub acc_freq: u8, +} + +impl StateEvent { + pub fn build(&self) -> Event { + Event::new("StateEvent", self.clone()) + } +} + +pub enum TestMachineStepperEvents { + State(Event), +} + +#[derive(Deserialize)] +#[serde(tag = "action", content = "value")] +pub enum Mutation { + SetTargetSpeed { target: i16 }, + SetEnabled { enabled: bool }, + SetFreq { factor: u8 }, + SetAccFreq { factor: u8 }, +} + +#[derive(Debug, Clone)] +pub struct TestMachineStepperNamespace { + pub namespace: Option, +} + +impl NamespaceCacheingLogic for TestMachineStepperNamespace { + fn emit(&mut self, events: TestMachineStepperEvents) { + let event = Arc::new(events.event_value()); + let buffer_fn = events.event_cache_fn(); + if let Some(ns) = &mut self.namespace { + ns.emit(event, &buffer_fn); + } + } +} + +impl CacheableEvents for TestMachineStepperEvents { + fn event_value(&self) -> GenericEvent { + match self { + TestMachineStepperEvents::State(event) => event.clone().into(), + } + } + + fn event_cache_fn(&self) -> CacheFn { + cache_first_and_last_event() + } +} + +impl MachineApi for TestMachineStepper { + fn api_get_sender(&self) -> smol::channel::Sender { + self.api_sender.clone() + } + + fn api_mutate(&mut self, request_body: Value) -> Result<(), anyhow::Error> { + let mutation: Mutation = serde_json::from_value(request_body)?; + match mutation { + Mutation::SetTargetSpeed { target } => self.set_target_speed(target), + Mutation::SetEnabled { enabled } => self.set_enabled(enabled), + Mutation::SetFreq { factor } => self.set_freq(factor), + Mutation::SetAccFreq { factor } => self.set_acc_freq(factor), + } + + Ok(()) + } + + fn api_event_namespace(&mut self) -> Option { + self.namespace.namespace.clone() + } +} diff --git a/machines/src/test_machine_stepper/mod.rs b/machines/src/test_machine_stepper/mod.rs new file mode 100644 index 00000000..1128f3ba --- /dev/null +++ b/machines/src/test_machine_stepper/mod.rs @@ -0,0 +1,76 @@ +use crate::machine_identification::{MachineIdentification, MachineIdentificationUnique}; +use crate::test_machine_stepper::api::{StateEvent, TestMachineStepperEvents}; +use crate::{AsyncThreadMessage, Machine, MachineMessage}; +use control_core::socketio::namespace::NamespaceCacheingLogic; +use ethercat_hal::io::stepper_velocity_wago_750_672::StepperVelocityWago750672; +use smol::channel::{Receiver, Sender}; +use std::time::Instant; +pub mod act; +pub mod api; +pub mod new; +use crate::test_machine_stepper::api::TestMachineStepperNamespace; +use crate::{TEST_MACHINE_STEPPER, VENDOR_QITECH}; + +#[derive(Debug)] +pub struct TestMachineStepper { + pub api_receiver: Receiver, + pub api_sender: Sender, + pub machine_identification_unique: MachineIdentificationUnique, + pub namespace: TestMachineStepperNamespace, + pub last_state_emit: Instant, + pub main_sender: Option>, + pub stepper: StepperVelocityWago750672, +} + +impl Machine for TestMachineStepper { + fn get_machine_identification_unique(&self) -> MachineIdentificationUnique { + self.machine_identification_unique.clone() + } + + fn get_main_sender(&self) -> Option> { + self.main_sender.clone() + } +} +impl TestMachineStepper { + pub const MACHINE_IDENTIFICATION: MachineIdentification = MachineIdentification { + vendor: VENDOR_QITECH, + machine: TEST_MACHINE_STEPPER, + }; +} + +impl TestMachineStepper { + pub fn get_state(&self) -> StateEvent { + StateEvent { + target_speed: self.stepper.target_velocity, + enabled: self.stepper.enabled, + freq: self.stepper.freq_range_sel, + acc_freq: self.stepper.acc_range_sel, + } + } + + pub fn emit_state(&mut self) { + let event = self.get_state().build(); + + self.namespace.emit(TestMachineStepperEvents::State(event)); + } + + pub fn set_target_speed(&mut self, speed: i16) { + self.stepper.set_velocity(speed); + self.emit_state(); + } + + pub fn set_enabled(&mut self, enabled: bool) { + self.stepper.set_enabled(enabled); + self.emit_state(); + } + + pub fn set_freq(&mut self, factor: u8) { + self.stepper.set_freq_range_sel(factor); + self.emit_state(); + } + + pub fn set_acc_freq(&mut self, factor: u8) { + self.stepper.set_acc_range_sel(factor); + self.emit_state(); + } +} diff --git a/machines/src/test_machine_stepper/new.rs b/machines/src/test_machine_stepper/new.rs new file mode 100644 index 00000000..8dde792c --- /dev/null +++ b/machines/src/test_machine_stepper/new.rs @@ -0,0 +1,86 @@ +use crate::test_machine_stepper::{TestMachineStepper, api::TestMachineStepperNamespace}; +use ethercat_hal::{ + devices::{ + EthercatDevice, downcast_device, + wago_750_354::{WAGO_750_354_IDENTITY_A, Wago750_354}, + wago_modules::wago_750_672::Wago750_672, + }, + io::stepper_velocity_wago_750_672::StepperVelocityWago750672, +}; +use smol::{block_on, lock::RwLock}; +use std::{sync::Arc, time::Instant}; + +use crate::{ + MachineNewHardware, MachineNewParams, MachineNewTrait, get_ethercat_device, + validate_no_role_dublicates, validate_same_machine_identification_unique, +}; + +use anyhow::Error; + +impl MachineNewTrait for TestMachineStepper { + fn new<'maindevice>(params: &MachineNewParams) -> Result { + // validate general stuff + let device_identification = params + .device_group + .iter() + .map(|device_identification| device_identification.clone()) + .collect::>(); + validate_same_machine_identification_unique(&device_identification)?; + validate_no_role_dublicates(&device_identification)?; + + let hardware = match ¶ms.hardware { + MachineNewHardware::Ethercat(x) => x, + _ => { + return Err(anyhow::anyhow!( + "[{}::MachineNewTrait/TestMachineStepper::new] MachineNewHardware is not Ethercat", + module_path!() + )); + } + }; + block_on(async { + let _wago_750_354 = get_ethercat_device::( + hardware, + params, + 0, + [WAGO_750_354_IDENTITY_A].to_vec(), + ) + .await?; + + let modules = Wago750_354::initialize_modules(_wago_750_354.1).await?; + + let mut coupler = _wago_750_354.0.write().await; + + for module in modules { + coupler.set_module(module); + } + + coupler.init_slot_modules(_wago_750_354.1); + let dev = coupler.slot_devices.first().unwrap().clone().unwrap(); + + // change uncomment this to change to different stepper driver + // + // let wago_750_671: Arc> = + // downcast_device::(dev).await?; + let wago_750_672: Arc> = + downcast_device::(dev).await?; + drop(coupler); + + let stepper = StepperVelocityWago750672::new(wago_750_672); + + let (sender, receiver) = smol::channel::unbounded(); + let mut my_test = Self { + api_receiver: receiver, + api_sender: sender, + machine_identification_unique: params.get_machine_identification_unique(), + namespace: TestMachineStepperNamespace { + namespace: params.namespace.clone(), + }, + last_state_emit: Instant::now(), + main_sender: params.main_thread_channel.clone(), + stepper, + }; + my_test.emit_state(); + Ok(my_test) + }) + } +} diff --git a/nixos/packages/electron.nix b/nixos/packages/electron.nix index 250f2698..894236ab 100644 --- a/nixos/packages/electron.nix +++ b/nixos/packages/electron.nix @@ -10,7 +10,7 @@ buildNpmPackage rec { ELECTRON_SKIP_BINARY_DOWNLOAD = 1; makeCacheWritable = true; - npmDepsHash = "sha256-rnaAsSkoTRoBgLCTF8ZFpi7uUQ+8W+u3qoMxsptX6NU="; + npmDepsHash = "sha256-N2kdS9IKkEMNOPPnRNQ9GvCenR/k88fVyAFzXMJyxs4="; npmFlags = [ "--no-audit" "--no-fund" ]; installPhase = ''