From 5c061a56eac6106e39a2870507790ac64b24e2db Mon Sep 17 00:00:00 2001 From: CBS-81 <56771401+1019954@users.noreply.github.com> Date: Mon, 10 May 2021 12:39:37 -0700 Subject: [PATCH] Changed trajectory code --- build.gradle | 2 +- src/main/java/frc/robot/Robot.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 28 +++- src/main/java/frc/robot/subsystems/Arm.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 32 +++-- vendordeps/Phoenix.json | 134 +++++++++++------- 6 files changed, 141 insertions(+), 69 deletions(-) diff --git a/build.gradle b/build.gradle index 1023980..1e9dafc 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2021.1.2" + id "edu.wpi.first.GradleRIO" version "2021.3.1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c127f51..49db688 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,8 +8,13 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + //import frc.robot.subsystems.Arm; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -21,17 +26,19 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; - + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ - + @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable table = inst.getTable("datatable"); } /** @@ -105,6 +112,7 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + m_robotContainer.returnFinalPose(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba10a03..7433ae3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ 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.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; @@ -73,6 +74,10 @@ public RobotContainer() { private void configureButtonBindings() { } + public void returnFinalPose() { + SmartDashboard.putNumber("Final Pose", drivetrain.getPose().getRotation().getDegrees()); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -81,13 +86,22 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - + TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(2), Units.feetToMeters(2)); config.setKinematics(drivetrain.getKinematics()); - + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - Arrays.asList(new Pose2d(), new Pose2d(1.0, 0, new Rotation2d())), - config); + Arrays.asList(new Pose2d(),new Pose2d(0, 1.0, new Rotation2d(0))),config); + drivetrain.reset(trajectory); + + System.out.println("trajectory after reset: " + trajectory); + var transform = drivetrain.getPose().minus(trajectory.getInitialPose()); + System.out.println("transofrm: " + transform); + trajectory = trajectory.transformBy(transform); + System.out.println("trajectory: " + trajectory); + + SmartDashboard.putNumber("Initial Rotation", trajectory.getInitialPose().getRotation().getDegrees()); + SmartDashboard.putNumber("Pose", drivetrain.getPose().getRotation().getDegrees()); RamseteCommand command = new RamseteCommand( trajectory, @@ -101,7 +115,9 @@ public Command getAutonomousCommand() { drivetrain::setOutput, drivetrain ); - + //drivetrain.reset(trajectory); return command; - } + } + + } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 9610d68..0bdd5d8 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -20,7 +20,7 @@ public class Arm extends SubsystemBase { * Creates a new Arm. */ public Arm() { - + armMotor.setInverted(true); } public void turn(XboxController controller) { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 90c3316..9713adf 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -6,6 +6,7 @@ /*----------------------------------------------------------------------------*/ package frc.robot.subsystems; + //import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; @@ -16,6 +17,7 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,6 +27,7 @@ import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.SPI; public class Drivetrain extends SubsystemBase { @@ -33,28 +36,30 @@ public class Drivetrain extends SubsystemBase { CANSparkMax leftMotor = new CANSparkMax(Constants.MOTOR_LEFT_ID, MotorType.kBrushless); CANSparkMax rightMotor = new CANSparkMax(Constants.MOTOR_RIGHT_ID, MotorType.kBrushless); + //NavX gyroscope AHRS gyro = new AHRS(SPI.Port.kMXP); // how robot need to go - DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(26)); + DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(0.527); // how robot positioned DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(getHeading()); - SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.4, 1, 0.4); + SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.323, 3.71, 0.313); // how robot turn wheel - PIDController leftPIDController = new PIDController(0.1,0.0,0.05); - PIDController rightPIDController = new PIDController(0.1,0.0,0.05); + PIDController leftPIDController = new PIDController(1.84,0.0,0.0); + PIDController rightPIDController = new PIDController(1.84,0.0,0.0); - Pose2d pose; + Pose2d pose = new Pose2d(); DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); /** * Creates a new Drivetrain. */ public Drivetrain() { - + //gyro.calibrate(); + //gyro.reset(); } /** @@ -68,7 +73,10 @@ public void driveWithJoysticks(XboxController controller) { public void periodic() { // This method will be called once per scheduler run pose = odometry.update(getHeading(), (0.0254 * (leftMotor.getEncoder().getPosition() * (1/10.75) * 6 * Math.PI)), (0.0254 * (rightMotor.getEncoder().getPosition() * (1/10.75) * 6 * Math.PI))); - SmartDashboard.putNumber("Gyro Angle", -gyro.getAngle()); + SmartDashboard.putNumber("Gyro Angle", -gyro.getAngle()); + SmartDashboard.putNumber("Left Encoder", leftMotor.getEncoder().getPosition()); + SmartDashboard.putNumber("Right Encoder", rightMotor.getEncoder().getPosition()); + } /** @@ -76,7 +84,7 @@ public void periodic() { * @return Angle of the gyroscope on a 2d plane in degrees */ public Rotation2d getHeading() { - return Rotation2d.fromDegrees(-gyro.getAngle()); + return Rotation2d.fromDegrees(0); } /** @@ -141,7 +149,13 @@ public PIDController getRightPIDController() { */ public void setOutput(double leftVolts, double rightVolts) { leftMotor.set(leftVolts / 12); - rightMotor.set(rightVolts / 12); + rightMotor.set( rightVolts / 12); } + public void reset(Trajectory trajectory){ + //gyro.reset(); + odometry.resetPosition(trajectory.getInitialPose(), getHeading()); + leftMotor.getEncoder().setPosition(0); + rightMotor.getEncoder().setPosition(0); + } } diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 4713c41..87f03cb 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,81 +1,69 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.18.3", + "version": "5.19.4", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ - "http://devsite.ctr-electronics.com/maven/release/" + "https://devsite.ctr-electronics.com/maven/release/" ], - "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "jsonUrl": "https://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.18.3" + "version": "5.19.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.18.3" + "version": "5.19.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.3", + "version": "5.19.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "diagnostics", - "version": "5.18.3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" + "linuxathena" ] }, { - "groupId": "com.ctre.phoenix", - "artifactId": "canutils", - "version": "5.18.3", + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { - "groupId": "com.ctre.phoenix", - "artifactId": "platform-stub", - "version": "5.18.3", + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.18.3", + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ - "linuxathena", "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] } ], @@ -83,7 +71,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -91,13 +79,14 @@ "binaryPlatforms": [ "linuxathena", "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -105,27 +94,40 @@ "binaryPlatforms": [ "linuxathena", "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "linuxathena", + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -133,39 +135,42 @@ "binaryPlatforms": [ "linuxathena", "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", - "artifactId": "platform-stub", - "version": "5.18.3", + "artifactId": "platform-sim", + "version": "5.19.4", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.18.3", + "version": "5.19.4", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, @@ -173,7 +178,36 @@ "binaryPlatforms": [ "linuxathena", "windowsx86-64", - "linuxx86-64" + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" ] } ]