From 2a3b80584097ed3c9089db216a5ab03ac08e9b2c Mon Sep 17 00:00:00 2001 From: Roman Semenov Date: Mon, 6 Mar 2023 21:36:16 -0800 Subject: [PATCH] Added romi support to simulate drivetrain on romi robots. + draft for odometry --- build.gradle | 7 +- .../java/frc/robot/subsystems/Drivetrain.java | 86 +++++++++++++------ vendordeps/RomiVendordep.json | 36 ++++++++ 3 files changed, 103 insertions(+), 26 deletions(-) create mode 100644 vendordeps/RomiVendordep.json diff --git a/build.gradle b/build.gradle index 3b15497..c5f7e57 100644 --- a/build.gradle +++ b/build.gradle @@ -43,7 +43,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true; // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -79,6 +79,11 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() +//Sets the websocket client remote host. +wpi.sim.envVar("HALSIMWS_HOST", "10.0.0.2") +wpi.sim.addWebsocketsServer().defaultEnabled = true +wpi.sim.addWebsocketsClient().defaultEnabled = true + // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f19d18e..d3fc87b 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -10,47 +10,80 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import frc.robot.Constants; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.romi.RomiGyro; public class Drivetrain extends SubsystemBase { /** Creates a new Drivetrain. */ - CANSparkMax rightFrontSpark; - CANSparkMax rightBackSpark; - CANSparkMax leftFrontSpark; - CANSparkMax leftBackSpark; - MotorControllerGroup rightMotors; - MotorControllerGroup leftMotors; - DifferentialDrive dDrive; - + CANSparkMax rightFrontSpark; + CANSparkMax rightBackSpark; + CANSparkMax leftFrontSpark; + CANSparkMax leftBackSpark; + MotorControllerGroup rightMotors; + MotorControllerGroup leftMotors; + DifferentialDrive dDrive; + DifferentialDriveOdometry m_odometry; + Pose2d m_pose; + + private Spark rightMotorRomi; + private Spark leftMotorRomi; + private Encoder leftEncoderRomi; + private Encoder rightEncoderRomi; + private RomiGyro gyroRomi; + public Drivetrain() { - rightFrontSpark = new CANSparkMax(Constants.RIGHT_FRONT_SPARK, MotorType.kBrushless); - rightBackSpark = new CANSparkMax(Constants.RIGHT_BACK_SPARK, MotorType.kBrushless); - leftFrontSpark = new CANSparkMax(Constants.LEFT_FRONT_SPARK, MotorType.kBrushless); - leftBackSpark = new CANSparkMax(Constants.LEFT_BACK_SPARK, MotorType.kBrushless); + if (RobotBase.isReal()) { + rightFrontSpark = new CANSparkMax(Constants.RIGHT_FRONT_SPARK, MotorType.kBrushless); + rightBackSpark = new CANSparkMax(Constants.RIGHT_BACK_SPARK, MotorType.kBrushless); + leftFrontSpark = new CANSparkMax(Constants.LEFT_FRONT_SPARK, MotorType.kBrushless); + leftBackSpark = new CANSparkMax(Constants.LEFT_BACK_SPARK, MotorType.kBrushless); + + rightBackSpark.follow(rightFrontSpark); + leftBackSpark.follow(leftFrontSpark); - rightBackSpark.follow(rightFrontSpark); - leftBackSpark.follow(leftFrontSpark); + rightFrontSpark.enableVoltageCompensation(12); + rightBackSpark.enableVoltageCompensation(12); + leftFrontSpark.enableVoltageCompensation(12); + leftBackSpark.enableVoltageCompensation(12); - rightFrontSpark.enableVoltageCompensation(12); - rightBackSpark.enableVoltageCompensation(12); - leftFrontSpark.enableVoltageCompensation(12); - leftBackSpark.enableVoltageCompensation(12); + rightFrontSpark.setIdleMode(IdleMode.kBrake); + rightBackSpark.setIdleMode(IdleMode.kBrake); + leftFrontSpark.setIdleMode(IdleMode.kBrake); + leftBackSpark.setIdleMode(IdleMode.kBrake); - rightFrontSpark.setIdleMode(IdleMode.kBrake); - rightBackSpark.setIdleMode(IdleMode.kBrake); - leftFrontSpark.setIdleMode(IdleMode.kBrake); - leftBackSpark.setIdleMode(IdleMode.kBrake); + rightMotors = new MotorControllerGroup(rightFrontSpark, rightBackSpark); + leftMotors = new MotorControllerGroup(leftFrontSpark, leftBackSpark); + } else { + // PWM channels 0 and 1 respectively + leftMotorRomi = new Spark(0); + rightMotorRomi = new Spark(1); + + rightMotors = new MotorControllerGroup(rightMotorRomi); + leftMotors = new MotorControllerGroup(leftMotorRomi); + leftEncoderRomi = new Encoder(4, 5); + rightEncoderRomi = new Encoder(6, 7); + + gyroRomi = new RomiGyro(); + + } - rightMotors = new MotorControllerGroup(rightFrontSpark, rightBackSpark); - leftMotors = new MotorControllerGroup(leftFrontSpark, leftBackSpark); dDrive = new DifferentialDrive(leftMotors, rightMotors); + //m_odometry = new DifferentialDriveOdometry( + // gyroSim.getRotation2d(), + // leftEncoderSim.getDistance(), rightEncoderSim.getDistance()); - } + public void arcadeDrive(double speed, double rotation){ dDrive.arcadeDrive(speed, rotation); @@ -63,5 +96,8 @@ public void curvatureDrive(double speed, double rotation){ @Override public void periodic() { // This method will be called once per scheduler run + //m_pose = m_odometry.update(gyroSim.getRotation2d(), + // leftEncoderSim.getDistance(), + // rightEncoderSim.getDistance()); } } \ No newline at end of file diff --git a/vendordeps/RomiVendordep.json b/vendordeps/RomiVendordep.json new file mode 100644 index 0000000..09c1ea1 --- /dev/null +++ b/vendordeps/RomiVendordep.json @@ -0,0 +1,36 @@ +{ + "fileName": "RomiVendordep.json", + "name": "Romi-Vendordep", + "version": "2022.2.1", + "uuid": "2854af28-a682-4c3e-abe5-1f62c288c6b8", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.romi", + "artifactId": "romi-vendordep-java", + "version": "2022.2.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.romi", + "artifactId": "romi-vendordep-cpp", + "version": "2022.2.1", + "libName": "romiVendordep", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file