编码器-软件

注解

本节介绍软件中的编码器。有关编码器的硬件指南,请参阅:ref:docs/hardware/sensors/encoders-hardware:Encoders - Hardware

Quadrature Encoders determine direction by observing which pulse channel (A or B) receives a pulse first.

编码器是用于测量运动(通常是轴的旋转)的设备。FRC中使用的编码器称为“正交编码器”。这些编码器在两个相位相差四分之一周期(因此称为“正交”)的通道上产生方波信号。脉冲用于测量旋转,可以从其中一个通道“引导”另一个通道时来确定运动方向。

A Quadrature Decoder analyzing the A, B, and Index signals.

FPGA通过计数器模块或编码器模块处理编码器,具体取决于解码类型-该选择由WPILib自动处理。FPGA包含8个编码器模块。

Encoder类

WPILib通过Encoder类(Java, C++)为编码器提供支持。此类提供了用于配置和读取编码器数据的简单API。

重要

``Encoder’’类仅用于直接插入roboRIO的编码器!请参阅适当的供应商文档,以了解如何使用插入电动机控制器的编码器。

初始化编码器

编码器可以实例化如下:

// Initializes an encoder on DIO pins 0 and 1
// Defaults to 4X decoding and non-inverted
Encoder encoder = new Encoder(0, 1);

解码类型

WPILib Encoder类可以以三种不同的模式解码编码器信号:

  • 1X解码:在编码器信号的每个完​​整周期(每四个边沿一次)增加距离。

  • 2X解码:每增加一半的编码器信号距离(每两个边沿增加一次)。

  • 4X解码:增加编码器信号每个边缘的距离(每个周期四次)。

4X解码可提供最高的精度,但可能会增加速率测量中的“抖动”。要使用其他解码类型,请使用以下构造函数:

// Initializes an encoder on DIO pins 0 and 1
// 2X encoding and non-inverted
Encoder encoder = new Encoder(0, 1, false, Encoder.EncodingType.k2X);

配置编码器参数

注解

该Encoder类不对距离单位做任何假设;它将以用于计算每脉冲距离值的任何单位返回值。因此,用户可以决定所使用的距离单位。但是,时间始终以秒为单位。

注解

计算每个脉冲的距离时使用的脉冲的数量不依赖于解码类型-每个“脉冲”应始终被认为是一个完整的周期(4条边)。

Encoder类提供了许多配置方法:

// Configures the encoder to return a distance of 4 for every 256 pulses
// Also changes the units of getRate
encoder.setDistancePerPulse(4./256.);

// Configures the encoder to consider itself stopped after .1 seconds
encoder.setMaxPeriod(.1);

// Configures the encoder to consider itself stopped when its rate is below 10
encoder.setMinRate(10);

// Reverses the direction of the encoder
encoder.setReverseDirection(true);

// Configures an encoder to average its period measurement over 5 samples
// Can be between 1 and 127 samples
encoder.setSamplesToAverage(5);

从编码器读取信息

Encoder类提供了丰富的编码器的运动信息给用户。

距离

注解

正交编码器测量相对距离,而不是绝对距离;返回的距离值取决于打开机器人电源或上次重置编码器值时编码器的位置。

用户可以使用以下getDistance()方法获得编码器行进的总距离:

// Gets the distance traveled
encoder.getDistance();

速率

注解

Encoder类的时间总是以秒为单位。

用户可以通过以下getRate()方法获得编码器的当前变化率:

// Gets the current rate of the encoder
encoder.getRate();

是否停止

用户可以通过以下getStopped()方法获取编码器是否停止:

// Gets whether the encoder is stopped
encoder.getStopped();

方向

用户可以使用以下getDirection()方法获取编码器最后移动的方向:

// Gets the last direction in which the encoder moved
encoder.getDirection();

周期

用户可以使用以下getPeriod()方法获得编码器脉冲的周期(以秒为单位):

// Gets the current period of the encoder
encoder.getPeriod();

重置编码器

要将编码器距离读数重置为零,请调用该reset()方法。这对于确保测得的距离与实际距离相对应很有用,并且经常在归位例程中调用该方法:

// Resets the encoder to read a distance of zero
encoder.reset();

在代码中使用编码器

编码器是FRC中最有用的传感器。他们能使机器人具有非凡的运动能力。编码器在机器人代码中的潜在应用太多,因此无法在此处进行全面总结,但下面提供了一些基本示例:

行驶一段距离

编码器可用于机器人底盘上,以创建简单的“行驶一段距离”例程。这对于机器人自动化非常有用:

// Creates an encoder on DIO ports 0 and 1
Encoder encoder = new Encoder(0, 1);

// Initialize motor controllers and drive
Spark left1 new Spark(0);
Spark left2 = new Spark(1);

Spark right1 = new Spark(2);
Spark right2 = new Spark(3);

SpeedControllerGroup leftMotors = new SpeedControllerGroup(left1, left2);
SpeedControllerGroup rightMotors = new SpeedControllerGroup(right1, right2);

DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);

@Override
public void robotInit() {
    // Configures the encoder's distance-per-pulse
    // The robot moves forward 1 foot per encoder rotation
    // There are 256 pulses per encoder rotation
    encoder.setDistancePerPulse(1./256.);
}

@Override
public void autonomousPeriodic() {
    // Drives forward at half speed until the robot has moved 5 feet, then stops:
    if(encoder.getDistance() < 5) {
        drive.tankDrive(.5, .5);
    } else {
        drive.tankDrive(0, 0);
    }
}

稳定航向

警告

与所有控制回路一样,用户应注意确保传感器方向和旋转方向一致。如果不是这样,则回路将变得不稳定,并且机器人将疯狂旋转。

编码器可以用与陀螺仪十分类似的方式确保机器人直线行驶。下面给出一个带有P回路的简单实现:

// The encoders for the drive
Encoder leftEncoder = new Encoder(0,1);
Encoder rightEncoder = new Encoder(2,3);

// The gain for a simple P loop
double kP = 1;

// Initialize motor controllers and drive
Spark left1 = new Spark(0);
Spark left2 = new Spark(1);

Spark right1 = new Spark(2);
Spark right2 = new Spark(3);

SpeedControllerGroup leftMotors = new SpeedControllerGroup(left1, left2);
SpeedControllerGroup rightMotors = new SpeedControllerGroup(right1, right2);

DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);

@Override
public void autonomousInit() {
    // Configures the encoders' distance-per-pulse
    // The robot moves forward 1 foot per encoder rotation
    // There are 256 pulses per encoder rotation
    leftEncoder.setDistancePerPulse(1./256.);
    rightEncoder.setDistancePerPulse(1./256.);
}

@Override
public void autonomousPeriodic() {
    // Assuming no wheel slip, the difference in encoder distances is proportional to the heading error
    double error = leftEncoder.getDistance() - rightEncoder.getDistance();

    // Drives forward continuously at half speed, using the encoders to stabilize the heading
    drive.tankDrive(.5 + kP * error, .5 - kP * error);
}

更高级的实现可以使用更复杂的控制回路。在编码器差异上闭合控制回路大致类似于在航向误差上闭合控制环路,因此PD回路特别有效。

PID控制

编码器作为PID控制器的输入特别有用(上面的航向稳定示例是一个简单的P回路)。

归位编码机制

由于编码器会测量相对距离,因此确保其“零点”在正确的位置通常很重要。一种典型的实现方法是“归位例程”,在该过程中,将机械装置移动到到达已知位置(通常由限位开关完成)或“原点”,然后重置编码器。以下代码提供了一个基本示例:

Encoder encoder = new Encoder(0, 1);

Spark spark = new Spark(0);

// Limit switch on DIO 2
DigitalInput limit = new DigitalInput(2);

public void autonomousPeriodic() {
    // Runs the motor backwards at half speed until the limit switch is pressed
    // then turn off the motor and reset the encoder
    if(!limit.get()) {
        spark.set(-.5);
    } else {
        spark.set(0);
        encoder.reset();
    }
}