Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Add simple differential drive simulation example (#2918)

Browse Source 
This provides an example of using the differential drive simulator without needing to use the command-based library.
This commit is contained in:
Prateek Machiraju
2020-12-08 01:32:42 -05:00
committed by GitHub
parent 4f40d991ea
commit 558e37c412
8 changed files with 539 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -632,6 +632,23 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "SimpleDifferentialDriveSimulation",
"description": "An example of a minimal drivetrain simulation project without the command-based library.",
"tags": [
"Drivetrain",
"State space",
"Digital",
"Sensors",
"Actuators",
"Joystick",
"Simulation"
],
"foldername": "simpledifferentialdrivesimulation",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "StateSpaceDriveSimulation",
"description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",

144
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java Normal file
View File

@@ -0,0 +1,144 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.numbers.N2;
@SuppressWarnings("PMD.TooManyFields")
public class Drivetrain {
// 3 meters per second.
public static final double kMaxSpeed = 3.0;
// 1/2 rotation per second.
public static final double kMaxAngularSpeed = Math.PI;
private static final double kTrackWidth = 0.381 * 2;
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = -4096;
private final PWMVictorSPX m_leftLeader = new PWMVictorSPX(1);
private final PWMVictorSPX m_leftFollower = new PWMVictorSPX(2);
private final PWMVictorSPX m_rightLeader = new PWMVictorSPX(3);
private final PWMVictorSPX m_rightFollower = new PWMVictorSPX(4);
private final SpeedControllerGroup m_leftGroup
= new SpeedControllerGroup(m_leftLeader, m_leftFollower);
private final SpeedControllerGroup m_rightGroup
= new SpeedControllerGroup(m_rightLeader, m_rightFollower);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry
= new DifferentialDriveOdometry(m_gyro.getRotation2d());
// Gains are for example purposes only - must be determined for your own
// robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
// Simulation classes help us simulate our robot
private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
private final Field2d m_fieldSim = new Field2d();
private final LinearSystem<N2, N2, N2> m_drivetrainSystem
= LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
private final DifferentialDrivetrainSim m_drivetrainSimulator
= new DifferentialDrivetrainSim(m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth,
kWheelRadius, null);
public Drivetrain() {
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
m_rightGroup.setInverted(true);
SmartDashboard.putData("Field", m_fieldSim);
}
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
speeds.leftMetersPerSecond);
double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
speeds.rightMetersPerSecond);
m_leftGroup.setVoltage(leftOutput + leftFeedforward);
m_rightGroup.setVoltage(rightOutput + rightFeedforward);
}
public void drive(double xSpeed, double rot) {
setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
}
public void updateOdometry() {
m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
m_rightEncoder.getDistance());
}
public void resetOdometry(Pose2d pose) {
m_leftEncoder.reset();
m_rightEncoder.reset();
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
}
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
public void simulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro. We negate the right side so that positive
// voltages make the right side move forward.
m_drivetrainSimulator.setInputs(m_leftLeader.get() * RobotController.getInputVoltage(),
-m_rightLeader.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

83
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java Normal file
View File

@@ -0,0 +1,83 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import java.util.List;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.SlewRateLimiter;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
public class Robot extends TimedRobot {
private final XboxController m_controller = new XboxController(0);
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final Drivetrain m_drive = new Drivetrain();
private final RamseteController m_ramsete = new RamseteController();
private final Timer m_timer = new Timer();
private Trajectory m_trajectory;
@Override
public void robotInit() {
m_trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(2, 2, new Rotation2d()), List.of(), new Pose2d(6, 4, new Rotation2d()),
new TrajectoryConfig(2, 2)
);
}
@Override
public void autonomousInit() {
m_timer.reset();
m_timer.start();
m_drive.resetOdometry(m_trajectory.getInitialPose());
}
@Override
public void autonomousPeriodic() {
double elapsed = m_timer.get();
Trajectory.State reference = m_trajectory.sample(elapsed);
ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference);
m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
m_drive.updateOdometry();
}
@Override
@SuppressWarnings("LocalVariableName")
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
double xSpeed = -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
* Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
double rot = -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
* Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
@Override
public void simulationPeriodic() {
m_drive.simulationPeriodic();
}
}