From 4de4d896fc79cb55975e410f9b3179070d5036f6 Mon Sep 17 00:00:00 2001 From: x64-bit <40089761+x64-bit@users.noreply.github.com> Date: Tue, 4 May 2021 14:24:30 -0700 Subject: [PATCH] Commented out non-drivetrain motors still doesn't work --- build.gradle | 4 +-- src/main/java/frc/robot/RobotContainer.java | 7 +++-- .../java/frc/robot/commands/ArcadeDrive.java | 1 - src/main/java/frc/robot/commands/Roll.java | 1 - src/main/java/frc/robot/subsystems/Arm.java | 4 +-- .../java/frc/robot/subsystems/Drivetrain.java | 31 ++++++++++--------- .../java/frc/robot/subsystems/Roller.java | 4 +-- src/main/java/frc/robot/subsystems/Winch.java | 8 ++--- 8 files changed, 31 insertions(+), 29 deletions(-) diff --git a/build.gradle b/build.gradle index 1023980..1988f06 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 @@ -38,7 +38,7 @@ deploy { } // 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 4. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 192fd1a..1d0c259 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,9 +86,12 @@ public Command getAutonomousCommand() { config.setKinematics(drivetrain.getKinematics()); Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - Arrays.asList(new Pose2d(), new Pose2d(1.0, 0, new Rotation2d())), + Arrays.asList(new Pose2d(), + new Pose2d(0, 1.0, new Rotation2d(0)), + new Pose2d(0, 1.0, new Rotation2d(90)), + new Pose2d(1.0, 1.0, new Rotation2d(90))), config); - + RamseteCommand command = new RamseteCommand( trajectory, drivetrain::getPose, diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index 0be2241..085babc 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -31,7 +31,6 @@ public void initialize() { @Override public void execute() { dt.driveWithJoysticks(RobotContainer.controller); - System.out.print("Drive"); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Roll.java b/src/main/java/frc/robot/commands/Roll.java index cf8b760..3b733b8 100644 --- a/src/main/java/frc/robot/commands/Roll.java +++ b/src/main/java/frc/robot/commands/Roll.java @@ -31,7 +31,6 @@ public void initialize() { @Override public void execute() { succRoller.intake(RobotContainer.controller); - System.out.print("Execute"); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 9610d68..81bfa0a 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -15,7 +15,7 @@ import frc.robot.Constants; public class Arm extends SubsystemBase { - TalonSRX armMotor = new TalonSRX(Constants.MOTOR_ARM_ID); + // TalonSRX armMotor = new TalonSRX(Constants.MOTOR_ARM_ID); /** * Creates a new Arm. */ @@ -24,7 +24,7 @@ public Arm() { } public void turn(XboxController controller) { - armMotor.set(ControlMode.PercentOutput, controller.getRawAxis(Constants.JOYSTICK_ARM_CONTROL_ID)); + // armMotor.set(ControlMode.PercentOutput, controller.getRawAxis(Constants.JOYSTICK_ARM_CONTROL_ID)); } @Override diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f8f1e76..e4efefa 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -77,28 +77,29 @@ public class Drivetrain extends SubsystemBase { DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(getHeading()); // Feedforward controller for Ramsete - SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.4, 1, 0.4); + SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.137, 3.24, 0.592); // 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(2.21,0.0,0.0); + PIDController rightPIDController = new PIDController(2.21,0.0,0.0); Pose2d pose; DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); //Create drivetrain simulation - static final double KvLinear = 0; - static final double KaLinear = 0; - static final double KvAngular = 0; - static final double KaAngular = 0; + static final double KvLinear = 3.24; + static final double KaLinear = 0.592; + static final double KvAngular = 1.5; + static final double KaAngular = 0.3; + static final double trackWidthMeters = 0.51; private DifferentialDrivetrainSim m_driveSim = new DifferentialDrivetrainSim( LinearSystemId.identifyDrivetrainSystem(KvLinear, KaLinear, KvAngular, KaAngular), DCMotor.getNEO(2), - 0.025, - 10.75, - Units.inchesToMeters(3), + 10.75, + 0.509, + Units.inchesToMeters(3), // The standard deviations for measurement noise: // x and y: 0.001 m @@ -132,13 +133,13 @@ public void simulationPeriodic(){ // subsystem in a separate thread or have changed the nominal timestep // of TimedRobot, this value needs to match it. - m_driveSim.update(0.02); + m_driveSim.update(0.07); //Update sensors - leftPos.set(m_driveSim.getLeftPositionMeters()); - leftVel.set(m_driveSim.getLeftVelocityMetersPerSecond()); - rightPos.set(m_driveSim.getRightPositionMeters()); - rightVel.set(m_driveSim.getRightVelocityMetersPerSecond()); + simEncoderLeft.getDouble("Position").set(m_driveSim.getLeftPositionMeters()); + simEncoderLeft.getDouble("Velocity").set(m_driveSim.getLeftVelocityMetersPerSecond()); + simEncoderRight.getDouble("Position").set(m_driveSim.getRightPositionMeters()); + simEncoderRight.getDouble("Velocity").set(m_driveSim.getRightVelocityMetersPerSecond()); //Update gyro simAngle.set(-m_driveSim.getHeading().getDegrees()); } diff --git a/src/main/java/frc/robot/subsystems/Roller.java b/src/main/java/frc/robot/subsystems/Roller.java index 41b21a3..b868261 100644 --- a/src/main/java/frc/robot/subsystems/Roller.java +++ b/src/main/java/frc/robot/subsystems/Roller.java @@ -14,7 +14,7 @@ public class Roller extends SubsystemBase { - VictorSPX rollerMotor = new VictorSPX(Constants.MOTOR_ROLLER_ID); + // VictorSPX rollerMotor = new VictorSPX(Constants.MOTOR_ROLLER_ID); /** * Creates a new Roller. */ @@ -37,7 +37,7 @@ public void intake(XboxController controller) { */ double intake = controller.getRawAxis(Constants.JOYSTICK_INTAKE_ID); double output = controller.getRawAxis(Constants.JOYSTICK_OUTPUT_ID); - rollerMotor.set(ControlMode.PercentOutput, intake - output); + // rollerMotor.set(ControlMode.PercentOutput, intake - output); } @Override diff --git a/src/main/java/frc/robot/subsystems/Winch.java b/src/main/java/frc/robot/subsystems/Winch.java index fee7cac..0eab5f1 100644 --- a/src/main/java/frc/robot/subsystems/Winch.java +++ b/src/main/java/frc/robot/subsystems/Winch.java @@ -7,7 +7,7 @@ package frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +// import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -16,14 +16,14 @@ public class Winch extends SubsystemBase { /** * Creates a new Winch. */ - TalonSRX winchMotor = new TalonSRX(Constants.MOTOR_CLIMB_WINCH_ID); + // TalonSRX winchMotor = new TalonSRX(Constants.MOTOR_CLIMB_WINCH_ID); public void winchUp() { - winchMotor.set(ControlMode.PercentOutput, 1.0); + // winchMotor.set(ControlMode.PercentOutput, 1.0); } public void winchDown() { - winchMotor.set(ControlMode.PercentOutput, -1.0); + // winchMotor.set(ControlMode.PercentOutput, -1.0); } @Override