3. Adım: Bir Sürücü Alt Sistemi Oluşturma

Artık sürücümüzün özelliği belirlendiğine göre, robot kodumuzu doğru yazmaya başlamanın zamanı geldi. Daha önce de belirtildiği gibi, robot kodumuz için komut tabanlı çerçevesini kullanacağız. Buna göre, ilk adımımız uygun bir sürücü yazmaktır subsystem ‘sınıfı.

RamseteCommand Örnek Projesindeki tam sürücü sınıfı (Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand> __, C ++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/RamseteCommand> __) aşağıda görülebilir. Makalenin geri kalanı bu dersi yazmanın adımlarını açıklayacak.

  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;

public class DriveSubsystem extends SubsystemBase {
  // The motors on the left side of the drive.
  private final SpeedControllerGroup m_leftMotors =
      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));

  // The motors on the right side of the drive.
  private final SpeedControllerGroup m_rightMotors =
      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));

  // The robot's drive
  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);

  // The left-side drive encoder
  private final Encoder m_leftEncoder =
      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
                  DriveConstants.kLeftEncoderReversed);

  // The right-side drive encoder
  private final Encoder m_rightEncoder =
      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
                  DriveConstants.kRightEncoderReversed);

  // The gyro sensor
  private final Gyro m_gyro = new ADXRS450_Gyro();

  // Odometry class for tracking robot pose
  private final DifferentialDriveOdometry m_odometry;

  /**
   * Creates a new DriveSubsystem.
   */
  public DriveSubsystem() {
    // Sets the distance per pulse for the encoders
    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

    resetEncoders();
    m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
  }

  @Override
  public void periodic() {
    // Update the odometry in the periodic block
    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
                      m_rightEncoder.getDistance());
  }

  /**
   * Returns the currently-estimated pose of the robot.
   *
   * @return The pose.
   */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

  /**
   * Returns the current wheel speeds of the robot.
   *
   * @return The current wheel speeds.
   */
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
  }

  /**
   * Resets the odometry to the specified pose.
   *
   * @param pose The pose to which to set the odometry.
   */
  public void resetOdometry(Pose2d pose) {
    resetEncoders();
    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
  }

  /**
   * Drives the robot using arcade controls.
   *
   * @param fwd the commanded forward movement
   * @param rot the commanded rotation
   */
  public void arcadeDrive(double fwd, double rot) {
    m_drive.arcadeDrive(fwd, rot);
  }

  /**
   * Controls the left and right sides of the drive directly with voltages.
   *
   * @param leftVolts  the commanded left output
   * @param rightVolts the commanded right output
   */
  public void tankDriveVolts(double leftVolts, double rightVolts) {
    m_leftMotors.setVoltage(leftVolts);
    m_rightMotors.setVoltage(-rightVolts);
    m_drive.feed();
  }

  /**
   * Resets the drive encoders to currently read a position of 0.
   */
  public void resetEncoders() {
    m_leftEncoder.reset();
    m_rightEncoder.reset();
  }

  /**
   * Gets the average distance of the two encoders.
   *
   * @return the average of the two encoder readings
   */
  public double getAverageEncoderDistance() {
    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
  }

  /**
   * Gets the left drive encoder.
   *
   * @return the left drive encoder
   */
  public Encoder getLeftEncoder() {
    return m_leftEncoder;
  }

  /**
   * Gets the right drive encoder.
   *
   * @return the right drive encoder
   */
  public Encoder getRightEncoder() {
    return m_rightEncoder;
  }

  /**
   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
   *
   * @param maxOutput the maximum output to which the drive will be constrained
   */
  public void setMaxOutput(double maxOutput) {
    m_drive.setMaxOutput(maxOutput);
  }

  /**
   * Zeroes the heading of the robot.
   */
  public void zeroHeading() {
    m_gyro.reset();
  }

  /**
   * Returns the heading of the robot.
   *
   * @return the robot's heading in degrees, from -180 to 180
   */
  public double getHeading() {
    return m_gyro.getRotation2d().getDegrees();
  }

  /**
   * Returns the turn rate of the robot.
   *
   * @return The turn rate of the robot, in degrees per second
   */
  public double getTurnRate() {
    return -m_gyro.getRate();
  }
}

Drive-Sürüş Kodlayıcılarını-Encoder Yapılandırma

Sürücü encoderleri, sürücünün her iki yanındaki tekerleklerin dönüşünü ölçer. Kodlayıcıları doğru şekilde yapılandırmak için iki şeyi belirtmemiz gerekir: kodlayıcıların takılı olduğu bağlantı noktaları ve kodlayıcı darbesi başına mesafe. Ardından, alt sistemi kullanan koddan kodlayıcı değerlerine erişime izin veren yöntemler yazmamız gerekir.

Encoder Portları

Kodlayıcı bağlantı noktaları, kodlayıcının constructtor’ında şu şekilde belirtilir:

38
39
40
41
42
43
44
45
  private final Encoder m_leftEncoder =
      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
                  DriveConstants.kLeftEncoderReversed);

  // The right-side drive encoder
  private final Encoder m_rightEncoder =
      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
                  DriveConstants.kRightEncoderReversed);

Darbe Başına Enkoder Mesafesi

Darbe başına mesafe, kodlayıcının setDistancePerPulse yöntemi çağrılarak belirlenir. WPILib Encoder sınıfı için “pulse”, tam bir kodlayıcı döngüsüne (yani dört kenar) karşılık gelir ve bu nedenle FRC-Karakterizasyon yapılandırmasında belirtilen değerin 1/4’ü olacaktır. Ayrıca mesafenin metre cinsinden ölçülmesi gerektiğini de unutmayın!

58
59
    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

Encoder Accessor Erişimci Yöntemi

Kodlayıcılar tarafından ölçülen değerlere erişmek için aşağıdaki yöntemi dahil ediyoruz:

Önemli

The returned velocities must be in meters! Because we configured the distance per pulse on the encoders above, calling getRate() will automatically apply the conversion factor from encoder units to meters. If you are not using WPILib’s Encoder class, you must perform this conversion either through the respective vendor’s API or by manually multiplying by a conversion factor.

82
83
84
85
86
87
88
89
  /**
   * Returns the current wheel speeds of the robot.
   *
   * @return The current wheel speeds.
   */
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
  }

Ölçülen kodlayıcı değerlerini daha sonra RamseteCommand sınıfıyla daha kolay entegrasyon için bir DifferentialDriveWheelSpeeds nesnesine bağlıyoruz.

Jiroskopu Yapılandırma

Jiroskop, robotun yönünün değişim oranını ölçer (daha sonra, robotun yönünün ilk açıldığı zamana göre bir ölçümünü sağlamak için entegre edilebilir). Örneğimizde, birkaç yıldır parça kitinde bulunan Analog Devices ADXRS450 FRC Gyro Board <https://www.analog.com/en/landing-pages/001/first.html> __ ‘ı kullanıyoruz:

48
  private final Gyro m_gyro = new ADXRS450_Gyro();

Jiroskop Accessor Yöntemi

Jiroskop tarafından ölçülen varolan yönelim yönüne erişmek için aşağıdaki yöntemi dahil ediyoruz:

173
174
175
176
177
178
179
180
  /**
   * Returns the heading of the robot.
   *
   * @return the robot's heading in degrees, from -180 to 180
   */
  public double getHeading() {
    return m_gyro.getRotation2d().getDegrees();
  }

Odemetriyi yapılandırma

Artık kodlayıcılarımızı ve jiroskopumuzu yapılandırdığımıza göre, konumunu kodlayıcı ve jiroskop okumalarından otomatik olarak hesaplamak için sürücü alt sistemimizi kurmanın zamanı geldi.

İlk olarak, DifferentialDriveOdometry sınıfınına üye bir örneğini oluşturuyoruz:

51
  private final DifferentialDriveOdometry m_odometry;

Odemetriyi Güncelleme

Odometri sınıfı, kodlayıcı ve jiroskoptan gelen yeni okumaları dahil etmek için düzenli olarak güncellenmelidir. Bunu, her main loop yinelemesinde otomatik olarak bir kez çağrılan alt sistemin periodic yöntemi içinde gerçekleştiriyoruz:

66
67
68
69
70
  public void periodic() {
    // Update the odometry in the periodic block
    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
                      m_rightEncoder.getDistance());
  }

Odometri Accessor Yöntemi

Robotun mevcut hesaplanan pozuna-konumuna erişmek için aşağıdaki yöntemi ekliyoruz:

72
73
74
75
76
77
78
79
  /**
   * Returns the currently-estimated pose of the robot.
   *
   * @return The pose.
   */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

Voltaj tabanlı Sürüş Methodu

Son olarak, SpeedController arayüzünün setVoltage() yöntemini kullanarak sürücünün her iki tarafına voltajı ayarlamamıza olanak tanıyan bir ek yöntem eklemeliyiz. Varsayılan WPILib sürücü sınıfı bu işlevselliği içermez, bu yüzden kendimiz yazmalıyız:

110
111
112
113
114
115
116
117
118
119
120
  /**
   * Controls the left and right sides of the drive directly with voltages.
   *
   * @param leftVolts  the commanded left output
   * @param rightVolts the commanded right output
   */
  public void tankDriveVolts(double leftVolts, double rightVolts) {
    m_leftMotors.setVoltage(leftVolts);
    m_rightMotors.setVoltage(-rightVolts);
    m_drive.feed();
  }

Normal set() yönteminden ziyade setVoltage() yöntemini kullanmak çok önemlidir, çünkü bu, çalışma sırasında pil “voltaj düşüşünü” otomatik olarak telafi edecektir. İleri besleme gerilimlerimiz fiziksel olarak anlamlı olduğundan (ölçülen karakterizasyon verilerine dayandıkları için), bu, doğruluklarını sağlamak için çok önemlidir.