Skip to content

Commit

Permalink
Merge pull request #33 from FRC1360/tuning-for-dcmp
Browse files Browse the repository at this point in the history 
Tuning for dcmp
  • Loading branch information
@MichGedd
MichGedd authored Apr 5, 2023
2 parents 253f722 + 9f686c4 commit eab1480
Show file tree
Hide file tree
Showing 22 changed files with 341 additions and 166 deletions.
25 changes: 15 additions & 10 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ public final class Constants {
public static final class Drivetrain {
public static final double DRIVE_MOTION_PROFILE_MAX_VELOCITY = 4.000;
public static final double DRIVE_MOTION_PROFILE_MAX_ACCELERATION = 3.550;
public static final double ROTATION_MOTION_PROFILE_MAX_VELOCITY = 90.0;
public static final double ROTATION_MOTION_PROFILE_MAX_ACCELERATION = 90.0;
public static final double ROTATION_MOTION_PROFILE_MAX_VELOCITY = 180.0;
public static final double ROTATION_MOTION_PROFILE_MAX_ACCELERATION = 180.0;
}
//GENERAL
public static double NEO_ENCODER_TICKS_PER_REV = 42;
Expand All @@ -65,7 +65,7 @@ public static final class Drivetrain {
public static final int SHOULDER_MOTOR_SLAVE = 51;
public static final int SHOULDER_ENCODER = 0;
//public static final double SHOULDER_ENCODER_OFFSET = 0.542;
public static final double SHOULDER_ENCODER_OFFSET = 0.207;
public static final double SHOULDER_ENCODER_OFFSET = 0.194;
public static final double SHOULDER_GEAR_RATIO = (11.0 / 52.0) * (30.0 / 68.0) * (12.0 / 60.0);
public static final double SHOULDER_MANUAL_OVERRIDE_RANGE = 20.0;
public static final double MAX_SHOULDER_ANGLE = 90.0;
Expand Down Expand Up @@ -117,17 +117,22 @@ public static final class Drivetrain {
// CONE_SCORE_HIGH_POSITION
public static final double CONE_SCORE_HIGH_POSITION_WRIST = -40.0;
public static final double CONE_SCORE_HIGH_POSITION_ARM = 19.0;
public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 40.0;
public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 44.0;

// CUBE_SCORE_HIGH_POSITION
public static final double CUBE_SCORE_HIGH_POSITION_WRIST = -40.0;
public static final double CUBE_SCORE_HIGH_POSITION_ARM = 19.0;
public static final double CUBE_SCORE_HIGH_POSITION_SHOULDER = 37.0;
public static final double CUBE_SCORE_HIGH_POSITION_WRIST = 130.0;
public static final double CUBE_SCORE_HIGH_POSITION_ARM = 5.0;
public static final double CUBE_SCORE_HIGH_POSITION_SHOULDER = -7.5;

// SCORE_MID_POSITION
public static final double SCORE_MID_POSITION_WRIST = -35.0;
public static final double SCORE_MID_POSITION_ARM = HOME_POSITION_ARM;
public static final double SCORE_MID_POSITION_SHOULDER = 30.0;
public static final double CONE_SCORE_MID_POSITION_WRIST = -35.0;
public static final double CONE_SCORE_MID_POSITION_ARM = HOME_POSITION_ARM;
public static final double CONE_SCORE_MID_POSITION_SHOULDER = 30.0;

// CUBE_SCORE_MID_POSITION
public static final double CUBE_SCORE_MID_POSITION_WRIST = 170.0;
public static final double CUBE_SCORE_MID_POSITION_ARM = 0.0;
public static final double CUBE_SCORE_MID_POSITION_SHOULDER = -48.0;

// SINGLE_SUBSTATION_POSITION
public static final double SINGLE_SUBSTATION_POSITION_WRIST = 150.0;
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,8 @@ public void autonomousInit() {
if (m_autonomousCommand != null) {
(m_robotContainer.getArmHomeCommand()
.andThen(m_robotContainer.getGoToZeroWristCommand())
.andThen(m_robotContainer.getShoulderZeroCommand())
.andThen(m_autonomousCommand)).schedule();
.andThen(m_robotContainer.getShoulderZeroCommand()))
.andThen(m_autonomousCommand).schedule();
} else {
(m_robotContainer.getArmHomeCommand()
.andThen(m_robotContainer.getGoToZeroWristCommand())
Expand Down Expand Up @@ -138,7 +138,7 @@ public void teleopInit() {
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
// m_robotContainer.shoulderSubsystem.updateSmartDashboard();
m_robotContainer.shoulderSubsystem.updateSmartDashboard();
// m_robotContainer.wristSubsystem.updateSmartDashboard();
// m_robotContainer.armSubsystem.updateSmartDashboard();

Expand Down
37 changes: 20 additions & 17 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
import frc.robot.commands.intake.ManualPutdownCommand;
import frc.robot.commands.shoulder.ShoulderGoToPositionCommand;
import frc.robot.commands.shoulder.ShoulderHoldCommand;
import frc.robot.commands.shoulder.ShoulderMoveManual;
import frc.robot.commands.vision.StrafeAlign;
import frc.robot.commands.wrist.WristGoToPositionCommand;
import frc.robot.commands.wrist.WristHoldCommand;
Expand All @@ -46,6 +47,7 @@
import frc.robot.subsystems.ShoulderSubsystem;
import frc.robot.subsystems.WristSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.LEDSubsystem;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -60,27 +62,28 @@ public class RobotContainer {

// The robot's subsystems and commands are defined here...
public final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem();
public final ShoulderSubsystem shoulderSubsystem = new ShoulderSubsystem(() -> operatorController.getRightY()*Constants.SHOULDER_MANUAL_OVERRIDE_RANGE, operatorController.leftBumper());
public final ShoulderSubsystem shoulderSubsystem = new ShoulderSubsystem(() -> operatorController.getRightY()*Constants.SHOULDER_MANUAL_OVERRIDE_RANGE, operatorController.rightBumper());
private final ShoulderSubsystem.ShoulderWristMessenger shoulderMessenger = shoulderSubsystem.new ShoulderWristMessenger();
public final WristSubsystem wristSubsystem = new WristSubsystem(shoulderMessenger, () -> operatorController.getLeftY()*Constants.WRIST_MANUAL_OVERRIDE_RANGE, operatorController.leftBumper());
public final ArmSubsystem armSubsystem = new ArmSubsystem(/*() -> operatorController.getRightTriggerAxis()*Constants.ARM_MANUAL_OFFSET_RANGE, operatorController.leftBumper()*/);
public final WristSubsystem wristSubsystem = new WristSubsystem(shoulderMessenger, () -> operatorController.getLeftY()*Constants.WRIST_MANUAL_OVERRIDE_RANGE, () -> false);
public final ArmSubsystem armSubsystem = new ArmSubsystem(() -> operatorController.getLeftY()*Constants.ARM_MANUAL_OFFSET_RANGE, operatorController.rightBumper());
private final ArmSubsystem.ArmShoulderMessenger armMessenger = armSubsystem.new ArmShoulderMessenger();
private final Vision vision = new Vision();
public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
public final LEDSubsystem ledSubsystem = new LEDSubsystem();

private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();

// private final RightSide2ConeAuto rightConeAuto = new RightSide2ConeAuto(m_drivetrainSubsystem);

// private final LeftSide2ConeAuto leftConeAuto = new LeftSide2ConeAuto(m_drivetrainSubsystem);
private final LeftSide2ConeAuto leftConeAuto = new LeftSide2ConeAuto(m_drivetrainSubsystem);

private final ConeHighAndBalanceAuto highConeAndBalanceAuto = new ConeHighAndBalanceAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger,
wristSubsystem, armSubsystem, intakeSubsystem, armMessenger);
private final ConeHighAndDriveAuto highConeAndDriveAuto = new ConeHighAndDriveAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger,
wristSubsystem, armSubsystem, intakeSubsystem, armMessenger);
wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem);

private final ConeScoreHighAuto highConeAuto = new ConeScoreHighAuto(shoulderSubsystem, shoulderMessenger, wristSubsystem,
armSubsystem, intakeSubsystem, armMessenger);
armSubsystem, intakeSubsystem, armMessenger, ledSubsystem);

private final Simulator sim = new Simulator(m_drivetrainSubsystem);

Expand All @@ -100,10 +103,12 @@ public RobotContainer() {
m_drivetrainSubsystem,
() -> -modifyAxis(left_controller.getY()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND,
() -> -modifyAxis(left_controller.getX()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND,
() -> modifyAxis(right_controller.getX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
() -> modifyAxis(right_controller.getX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND,
right_controller
));

shoulderSubsystem.setDefaultCommand(new ShoulderHoldCommand(shoulderSubsystem, armMessenger, () -> this.operatorController.getLeftTriggerAxis()));
//shoulderSubsystem.setDefaultCommand(new ShoulderMoveManual(shoulderSubsystem, () -> this.operatorController.getLeftY()));
wristSubsystem.setDefaultCommand(new WristHoldCommand(wristSubsystem, () -> this.operatorController.getLeftTriggerAxis()));
armSubsystem.setDefaultCommand(new ArmHoldCommand(this.armSubsystem));
intakeSubsystem.setDefaultCommand(new IntakeHoldCommand(this.intakeSubsystem));
Expand Down Expand Up @@ -137,12 +142,12 @@ private void configureButtonBindings() {

left_controller.button(1).onTrue(new InstantCommand(m_drivetrainSubsystem::zeroGyroscope));

operatorController.a().onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem)));
operatorController.y().onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem));
operatorController.b().onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger));
operatorController.x().onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger));
operatorController.povUp().onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean()));
operatorController.povDown().onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem));
operatorController.a().onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem)));
operatorController.y().onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem));
operatorController.b().onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, () -> operatorController.leftBumper().getAsBoolean()));
operatorController.x().onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem));
operatorController.povUp().onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean(), ledSubsystem));
operatorController.povDown().onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem, ledSubsystem));


new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.05)
Expand All @@ -161,11 +166,9 @@ private void configureButtonBindings() {
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
//return autoChooser.getSelected();
//return rightConeAuto;
//return leftConeAuto;
//return null;
//return new ConeScoreHighAuto(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, intakeSubsystem, armMessenger);
// return leftConeAuto;
//return null;
return autoChooser.getSelected();
//return highConeAndBalanceAuto;
}
Expand Down
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger;
import frc.robot.subsystems.DrivetrainSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.ShoulderSubsystem;
import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger;
import frc.robot.subsystems.WristSubsystem;
Expand All @@ -22,11 +23,12 @@ public class ConeHighAndDriveAuto extends SequentialCommandGroup {

public ConeHighAndDriveAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger,
WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake,
ArmShoulderMessenger armMessenger) {
ArmShoulderMessenger armMessenger,
LEDSubsystem ledSubsystem) {

addCommands(new AutoAssemblyConeHighScoreCommand(shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger),
// .raceWith(new DriveSpeed(dt, -0.5, 0.0)), // New - To be tested!!!!
new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger),
new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, ledSubsystem),
new Drive(dt, 8.0, 0.0)
.raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0))
.raceWith(new WristHoldCommand(wrist, () -> 0.0))
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/autos/ConeScoreHighAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import frc.robot.subsystems.ArmSubsystem;
import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.ShoulderSubsystem;
import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger;
import frc.robot.subsystems.WristSubsystem;
Expand All @@ -18,13 +19,13 @@ public class ConeScoreHighAuto extends SequentialCommandGroup {

public ConeScoreHighAuto(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger,
WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake,
ArmShoulderMessenger armMessenger)
ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem)
{
addCommands(
new AutoAssemblyConeHighScoreCommand(shoulder, shoulderWristMessenger, wrist,
arm, intake, armMessenger),
new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger,
wrist, arm, armMessenger)
wrist, arm, armMessenger, ledSubsystem)
/*
new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)
.raceWith(new WristHoldCommand(wrist, () -> 0.0))
Expand Down
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/autos/LeftSide2ConeAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.autos.basic.Direction;
import frc.robot.autos.basic.DriveAndRotate;
//import frc.robot.autos.builderSequences.CenterToScore;
import frc.robot.autos.procedures.GetAndScoreCone;
import frc.robot.subsystems.DrivetrainSubsystem;
Expand All @@ -11,10 +12,11 @@ public class LeftSide2ConeAuto extends SequentialCommandGroup {
public LeftSide2ConeAuto(DrivetrainSubsystem dt) {
// Note: Rotation doesn't work in the simulator! But something to think out in real world
// Relative to starting position
// addCommands(/* Command to score preload */
addCommands(/* Command to score preload */
new DriveAndRotate(dt, 0.0, 0.0, 90.0)
// new GetAndScoreCone(dt, Direction.FRONT_CENTER, Direction.LEFT),
// new CenterToScore(dt, Direction.RIGHT), // To reset
// new GetAndScoreCone(dt, Direction.RIGHT, Direction.RIGHT)
// );
);
}
}
Loading

0 comments on commit eab1480

Please sign in to comment.