Elevator height

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -88,5 +93,11 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
   ]
 }

src/main/deploy/pathplanner/settings.json

@@ -12,20 +12,20 @@
   "robotMass": 74.088,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
+  "driveWheelRadius": 0.047625,
+  "driveGearing": 6.75,
   "maxDriveSpeed": 5.45,
-  "driveMotorType": "krakenX60",
+  "driveMotorType": "NEO",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
-  "flModuleX": 0.273,
-  "flModuleY": 0.273,
-  "frModuleX": 0.273,
-  "frModuleY": -0.273,
-  "blModuleX": -0.273,
-  "blModuleY": 0.273,
-  "brModuleX": -0.273,
-  "brModuleY": -0.273,
+  "flModuleX": 0.311151,
+  "flModuleY": 0.3115,
+  "frModuleX": 0.311,
+  "frModuleY": -0.311,
+  "blModuleX": -0.311,
+  "blModuleY": 0.311,
+  "brModuleX": -0.311,
+  "brModuleY": -0.311,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []

src/main/deploy/swerve/modules/physicalproperties.json

@@ -18,7 +18,7 @@
     }
   },
   "rampRate": {
-    "drive": 1,
+    "drive": 0.6,
     "angle": 0.15
   }
 }

src/main/java/frc/robot/Constants.java

@@ -46,6 +46,34 @@ public static class DrivetrainConfig {
     public static final PIDConstants DRIVE_PID = new PIDConstants(0.5, 0, 0);
     public static final PIDConstants TURN_PID = new PIDConstants(0.5, 0, 0);
   }
+  public static class ElevatorConstants{
+    public static final double L1Height = 20;
+    public static final double L2Height = 62;
+    public static final double L3Height = 108;
+    public static final double L4Height = 193;
+    public static final double A1Height = 45;
+    public static final double A2Height = 72;
+  }
+  public static class tiltConstants{
+    public static double startingHeight = 15.25;
+    public static double endingHeight = 59.25;
+    public static double startingEncoder = 0;
+    public static double endingEncoder = 110;
+
+    public static double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder);
+
+    public static final double tiltOffset = -5.928567; //Please put this in degrees and change it according to what you measure.
+
+    public static final double tiltMinSpeed = 1;
+    public static final double tiltMaxSpeed = 2.5;
+
+    public static final double tiltL1 = 3.380949;
+    public static final double tiltL2 = 4.85710;
+    public static final double tiltL3 = 6.642862;
+    public static final double tiltL4 = 3.071426;
+    public static final double tiltA1 = 22.118935;
+    public static final double tiltA2 = 22.118935;
+  }
 
   public static class SystemConfig {
     public static final PIDController PIVOT_PID = new PIDController(0.1, 0, 0);
@@ -55,7 +83,7 @@ public static class SystemConfig {
     public static final double SHOOTER_TOLERANCE = 2; // deg
     public static final double ELEVATOR_TOLERANCE = 0.1; // m
 
-    public static final double ELEVATOR_SPEED = 0.5;
+    public static final double ELEVATOR_SPEED = 0.7;
     public static final double PIVOT_SPEED = 0.5;
     public static final double SHOOTER_SPEED = 0.5;
     public static final double GRABBER_SPEED = 0.5;
@@ -74,7 +102,7 @@ public static class SystemConfig {
     public static final double SHOOTER_INTAKE_TIME = 0.5;
     public static final double CORAL_INTAKE_TIME = 1.0;
     public static final double CORAL_OUTTAKE_TIME = 1.0;
-    public static final double GRABBER_TIME = 3.0;
+    public static final double GRABBER_TIME = 1.0;
 
     public static final double CLIMBER_DEFAULT_POSITION = 0;
     public static final double CLIMBER_CLIMB_POSITION = 0.5;
@@ -98,6 +126,7 @@ public static final class DrivebaseConstants
   public static class OperatorConstants
   {
 
+
     // Joystick Deadband
     public static final double DEADBAND        = 0.1;
     public static final double LEFT_Y_DEADBAND = 0.1;

src/main/java/frc/robot/Robot.java

@@ -53,7 +53,9 @@ public void robotInit()
     disabledTimer = new Timer();
 
     //start streaming the camera feed
-    CameraServer.startAutomaticCapture();
+    CameraServer.startAutomaticCapture(0);
+    CameraServer.startAutomaticCapture(1);
+
 
     if (isSimulation())
     {
@@ -106,14 +108,14 @@ public void disabledPeriodic()
   @Override
   public void autonomousInit()
   {
-    // m_robotContainer.setMotorBrake(true);
-    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    // // schedule the autonomous command (example)
-    // if (m_autonomousCommand != null)
-    // {
-    //   m_autonomousCommand.schedule();
-    // }
+    m_robotContainer.setMotorBrake(true);
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null)
+    {
+      m_autonomousCommand.schedule();
+    }
     return;
   }
 

src/main/java/frc/robot/RobotContainer.java

@@ -37,9 +37,11 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 // import frc.robot.subsystems.drive.DriveSubsystem;
@@ -58,6 +60,7 @@ public class RobotContainer
   public static ClimberSubsystem climber = new ClimberSubsystem();
   public static ElevatorSubsystem elevator = new ElevatorSubsystem();
   public static EndEffectorSubsystem endEffector = new EndEffectorSubsystem();
+  public static Intake intake = new Intake();
 
   //Define Controllers
   public static CommandXboxController driverController = new CommandXboxController(0);
@@ -72,10 +75,12 @@ public class RobotContainer
   SwerveInputStream driveAngularVelocity = SwerveInputStream.of(driveTrain.getSwerveDrive(),
                                                                 () -> driverController.getLeftY() * -1,
                                                                 () -> driverController.getLeftX() * -1)
-                                                            .withControllerRotationAxis(driverController::getRightX)
+                                                            .withControllerRotationAxis(() -> driverController.getRightX()*-1)
                                                             .deadband(OperatorConstants.DEADBAND)
                                                             .scaleTranslation(0.8)
-                                                            .allianceRelativeControl(true);
+                                                            .allianceRelativeControl(true)
+                                                            .cubeTranslationControllerAxis(true)
+                                                            .cubeRotationControllerAxis(true);
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -86,21 +91,74 @@ public RobotContainer()
     configureBindings();
     DriverStation.silenceJoystickConnectionWarning(true);
     autoChooser = AutoBuilder.buildAutoChooser("Pass The Line Auto");
-
+    SmartDashboard.putData("Auto Mode", autoChooser);
     Command driveFieldOrientedAnglularVelocity = driveTrain.driveFieldOriented(driveAngularVelocity);
 
     driveTrain.setDefaultCommand(driveFieldOrientedAnglularVelocity);
 
+    climber.setDefaultCommand(Commands.run(() -> {
+         // Driver Controls
+
+
+   if(driverController.rightTrigger().getAsBoolean()){
+    if(climber.lowerLimitReachedWinch()){
+      climber.setSpeed(0);
+      climber.resetEncoder();
+    }
+    else{
+    climber.setSpeed(3);
+    }
+  }
+   else if(driverController.leftTrigger().getAsBoolean()){
+    climber.setSpeed(-3);
+   }
+   else{
+    climber.setSpeed(0);
+   }
+
 
-    climber.setDefaultCommand(new RunCommand(() -> {
     }, climber));
 
-    elevator.setDefaultCommand(new RunCommand(() -> {
-      elevator.setSpeed(-operatorController.getLeftY()*.1); //Multiply by .1 for testing
+
+    intake.setDefaultCommand(new InstantCommand(() -> {}, intake));
+
+    elevator.setDefaultCommand(new InstantCommand(() -> {
+      double joystick = -operatorController.getLeftY();
+
+      if (Math.abs(joystick) < 0.1)
+      {
+        elevator.goToHeight(elevator.getLastSetpoint());
+      }
+      else
+      {
+        //if the lower limit's been reached don't allow them to go down but allow them to go up. 
+        if(elevator.lowerLimitReached() == true && joystick <= 0)  {
+          //Multiply by .1 for testing
+          elevator.resetEncoder();
+          elevator.setSpeed(0);
+        }else 
+        {
+            // elevator.setSpeed(joystick);
+            joystick = handleDeadband(joystick, 0.1);
+            elevator.goToHeight(elevator.getLastSetpoint() + joystick * Constants.ElevatorConstants.L2Height / 50.0 / 1.0);
+        }
+      }
     }, elevator));
 
-    endEffector.setDefaultCommand(new RunCommand(() -> {
-      endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1);  //We are using this to test, the .1 is to make it go slow
+    endEffector.setDefaultCommand(new InstantCommand(() -> {
+      if(elevator.lowerLimitReached() == true /*&& -operatorController.getLeftY() <= 0*/) {        
+        //if((endEffector.endEffectorTilt.getEncoder().getPosition() <= -.8)||(endEffector.endEffectorTilt.getEncoder().getPosition() >= 0 )){
+          //if(endEffector.atSetPoint() == false){
+            endEffector.goToTilt(-.4);
+          //}else{
+           // endEffector.setSpeedEndEffectorTilt(0);
+          //}
+        //}
+      }else if((elevator.elevator1.getEncoder().getPosition() <= 30)) {
+        endEffector.goToTilt(6);
+      }else{
+        endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow //this is the value to make sure the end effector clears the funnel
+      }
     }, endEffector));
   }
 
@@ -117,25 +175,71 @@ private void configureBindings()
     //To do: Set Buttons, Set Speeds, Verify Directions
 
     // Operator Controls
+    Command heightL1PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L1Height,elevator);
+    Command heightL2PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L2Height,elevator);
+    Command heightL3PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L3Height,elevator);
+    Command heightL4PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L4Height,elevator);
+    Command heightA1PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.A1Height,elevator);
+    Command heightA2PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.A2Height,elevator);
+
+    Command tiltL1PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL1, endEffector);
+    Command tiltL2PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL2, endEffector);
+    Command tiltL3PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL3, endEffector);
+    Command tiltL4PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL4, endEffector);
+    Command tiltA1PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA1, endEffector);
+    Command tiltA2PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA2, endEffector);
 
-    operatorController.rightTrigger().whileTrue(new ShootCommand(-.1));
-    operatorController.leftTrigger().whileTrue(new ShootCommand(.1));
+    SequentialCommandGroup goToPresetL1 = new SequentialCommandGroup(heightL1PresetCommand, tiltL1PresetCommand);
+    SequentialCommandGroup goToPresetL2 = new SequentialCommandGroup(heightL2PresetCommand, tiltL2PresetCommand);
+    SequentialCommandGroup goToPresetL3 = new SequentialCommandGroup(heightL3PresetCommand, tiltL3PresetCommand);
+    SequentialCommandGroup goToPresetL4 = new SequentialCommandGroup(heightL4PresetCommand, tiltL4PresetCommand);
+    SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand);
+    SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand);
 
-    operatorController.rightBumper().whileTrue(new IntakeCommand(-.1));
-    operatorController.leftBumper().whileTrue(new IntakeCommand(.1));
+    // operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5));
+    // operatorController.leftTrigger().whileTrue(new ShootCommand(2.5));
 
+    // operatorController.rightBumper().whileTrue(new IntakeCommand(-.2));
+    // operatorController.leftBumper().whileTrue(new IntakeCommand(.2));
 
-
-    // Driver Controls
-    driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();}));
-    driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();}));
+    operatorController.rightTrigger()
+    .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(-2.5), endEffector))
+    .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector));
 
-    driverController.rightTrigger().whileTrue(new ExtendWinchCommand(-.1));
-    driverController.leftTrigger().whileTrue(new ExtendWinchCommand(.1));
+    operatorController.leftTrigger()
+    .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(2.5), endEffector))
+    .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector));
 
-    driverController.rightBumper().whileTrue(new GrabCageCommand(.1));
-    driverController.leftBumper().whileTrue(new GrabCageCommand(-1.));
+
+    operatorController.rightBumper()
+    .onTrue(new InstantCommand(() -> intake.intake(-0.2), intake))
+    .onFalse(new InstantCommand(() -> intake.intake(0.0), intake));
+
+    operatorController.leftBumper()
+    .onTrue(new InstantCommand(() -> intake.intake(0.2), intake))
+    .onFalse(new InstantCommand(() -> intake.intake(0.0), intake));
+
+    operatorController.x().whileTrue(goToPresetL1);
+    operatorController.y().whileTrue(goToPresetL2);
+    operatorController.b().whileTrue(goToPresetL3);
+    operatorController.a().whileTrue(goToPresetL4);
+
+
+     operatorController.povDown().whileTrue(goToPresetA1);
+     operatorController.povUp().whileTrue(goToPresetA2);
+
+
+    driverController.rightBumper()
+    .onTrue(new InstantCommand(() -> climber.Grab(1.00), climber))
+    .onFalse(new InstantCommand(() -> climber.Grab(0.0), climber));
+
+    driverController.leftBumper()
+    .onTrue(new InstantCommand(() -> climber.Grab(-1.00), climber))
+    .onFalse(new InstantCommand(() -> climber.Grab(0.0), climber));
+
 
+    driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();}));
+    driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();}));
   }
   public void setMotorBrake(boolean brake)
   {
@@ -150,6 +254,19 @@ public Command getAutonomousCommand()
   {
     // Pass the auto line for points
     return autoChooser.getSelected();
+    // return driveTrain.getAutonomousCommand("Pass The Line Auto");
+  }
+
+
+  public static double handleDeadband(double value, double deadband)
+  {
+      deadband = Math.abs(deadband);
+      if (deadband == 1)
+      {
+          return 0;
+      }
+      double scaledValue = (value + (value < 0 ? deadband : -deadband)) / (1 - deadband);
+      return (Math.abs(value) > Math.abs(deadband)) ? scaledValue : 0;
   }
 }