Encoder Reading

Quadrature encoders provide precise position feedback using two phase-shifted signals (A and B). This example demonstrates:

• • Encoder configuration - Setting up quadrature inputs
• • Position tracking - Reading absolute and relative position
• • Velocity calculation - Computing speed from position changes
• • Direction detection - Determining rotation direction
• • Resolution modes - 1x, 2x, and 4x counting

use pokeys_lib::*;
use std::{thread, time::Duration};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    println!("🔄 PoKeys Encoder Reading Example");
    println!("=================================");
    
    // Connect to device
    let device_count = enumerate_usb_devices()?;
    if device_count == 0 {
        println!("❌ No PoKeys devices found!");
        return Ok(());
    }
    
    let mut device = connect_to_device(0)?;
    println!("✅ Connected to: {}", device.get_device_name()?);
    
    // Configure encoder 0 on pins 10 and 11 with 4x sampling.
    // Normal (per-encoder) EncoderOptions does not have an invert_direction
    // field — swap pins A and B if you need to reverse direction, or use the
    // fast / ultra-fast encoder APIs which expose a hardware invert flag.
    let encoder_options = EncoderOptions::with_4x_sampling();

    device.configure_encoder(0, 10, 11, encoder_options)?;
    println!("⚙️  Configured encoder 0 on pins 10 (A) and 11 (B)");
    
    // Reset encoder position to zero
    device.reset_encoder(0)?;
    println!("🔄 Reset encoder position to zero");
    
    println!("📊 Encoder readings (turn the encoder):");
    println!("Time | Position | Delta | Direction | Speed (counts/s)");
    println!("-----|----------|-------|-----------|----------------");
    
    let mut last_position = 0i32;
    let mut last_time = std::time::Instant::now();
    
    for i in 0..200 {
        // Read current encoder position
        let current_position = device.get_encoder_value(0)?;
        let current_time = std::time::Instant::now();
        
        // Calculate delta and speed
        let delta = current_position - last_position;
        let time_diff = current_time.duration_since(last_time).as_secs_f32();
        let speed = if time_diff > 0.0 { delta as f32 / time_diff } else { 0.0 };
        
        // Determine direction
        let direction = match delta {
            d if d > 0 => "CW ",
            d if d < 0 => "CCW",
            _ => "---",
        };
        
        println!("{:>4} | {:>8} | {:>5} | {:>9} | {:>13.1}", 
                 i, current_position, delta, direction, speed);
        
        last_position = current_position;
        last_time = current_time;
        
        thread::sleep(Duration::from_millis(100));
    }
    
    println!("🎉 Encoder reading completed!");
    Ok(())
}

use pokeys_lib::*;
use std::{collections::VecDeque, thread, time::{Duration, Instant}};

struct EncoderTracker {
    id: u8,
    name: String,
    position: i32,
    velocity_samples: VecDeque<f32>,
    last_position: i32,
    last_time: Instant,
    counts_per_revolution: i32,
}

impl EncoderTracker {
    fn new(id: u8, name: &str, counts_per_revolution: i32) -> Self {
        Self {
            id,
            name: name.to_string(),
            position: 0,
            velocity_samples: VecDeque::new(),
            last_position: 0,
            last_time: Instant::now(),
            counts_per_revolution,
        }
    }
    
    fn update(&mut self, device: &mut Device) -> Result<(), Box<dyn std::error::Error>> {
        let current_time = Instant::now();
        self.position = device.get_encoder_value(self.id)?;
        
        // Calculate velocity
        let delta_pos = self.position - self.last_position;
        let delta_time = current_time.duration_since(self.last_time).as_secs_f32();
        
        if delta_time > 0.0 {
            let velocity = delta_pos as f32 / delta_time;
            self.velocity_samples.push_back(velocity);
            
            // Keep only last 10 samples for smoothing
            if self.velocity_samples.len() > 10 {
                self.velocity_samples.pop_front();
            }
        }
        
        self.last_position = self.position;
        self.last_time = current_time;
        Ok(())
    }
    
    fn get_average_velocity(&self) -> f32 {
        if self.velocity_samples.is_empty() {
            return 0.0;
        }
        let sum: f32 = self.velocity_samples.iter().sum();
        sum / self.velocity_samples.len() as f32
    }
    
    fn get_revolutions(&self) -> f32 {
        self.position as f32 / self.counts_per_revolution as f32
    }
    
    fn get_rpm(&self) -> f32 {
        (self.get_average_velocity() / self.counts_per_revolution as f32) * 60.0
    }
}

fn multi_encoder_example() -> Result<(), Box<dyn std::error::Error>> {
    let mut device = connect_to_device(0)?;
    
    // Configure multiple encoders with 4x sampling.
    let encoder_options = EncoderOptions::with_4x_sampling();

    // Encoder 0: Rotary knob (24 counts/rev)
    device.configure_encoder(0, 10, 11, encoder_options)?;
    device.reset_encoder(0)?;
    
    // Encoder 1: Motor encoder (1000 counts/rev)
    device.configure_encoder(1, 12, 13, encoder_options)?;
    device.reset_encoder(1)?;
    
    println!("🔄 Multi-Encoder Position Control");
    println!("=================================");
    
    let mut knob = EncoderTracker::new(0, "Rotary Knob", 24);
    let mut motor = EncoderTracker::new(1, "Motor", 1000);
    
    println!("Knob: Pos | Rev | RPM  || Motor: Pos  | Rev  | RPM");
    println!("----------|-----|------||-------------|------|----");
    
    for _ in 0..300 {
        // Update both encoders
        knob.update(&mut device)?;
        motor.update(&mut device)?;
        
        println!("{:>9} |{:>4.1} |{:>5.0} || {:>10} |{:>5.2} |{:>4.0}",
                 knob.position,
                 knob.get_revolutions(),
                 knob.get_rpm(),
                 motor.position,
                 motor.get_revolutions(),
                 motor.get_rpm());
        
        thread::sleep(Duration::from_millis(100));
    }
    
    Ok(())
}