.OutlineViewer/outlineviewer.json

@@ -1,13 +0,0 @@
-{
-  "NetworkTables Settings": {
-    "mode": "Client (NT4)"
-  },
-  "transitory": {
-    "Shuffleboard": {
-      "Sensors Tab": {
-        "open": true
-      },
-      "open": true
-    }
-  }
-}

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

@@ -93,8 +93,7 @@ public class RobotContainer {
 			//elevator.runAssistedElevator(operator::getLeftY)
 	//	);
 
-		//elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+		elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
-		elevator.setDefaultCommand(elevator.manualControl(() -> 0.0));
 
 		manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
 
@@ -115,27 +114,10 @@ public class RobotContainer {
 			manipulator.runManipulator(() -> 1, true)
 		);
 
-		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.75)
-		);
-
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
 
-		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
-		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
-
-		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
-		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-
-		operator.povUp().whileTrue(elevator.goToSetpoint(30.0));
-
-		operator.a().whileTrue(elevator.manualControl(() -> 0.5));
-
-		operator.b().whileTrue(elevator.manualControl(() ->-0.1));
-
-		/* 
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
@@ -176,8 +158,6 @@ public class RobotContainer {
 		operator.b().onTrue(
 			algaeIntakeRoutine(false)
 		);
-		*/
-
 	}
 
 	private void configureNamedCommands() {
@@ -216,12 +196,6 @@ public class RobotContainer {
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
 
-		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue);
-
-		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch);
-
-		sensorTab.addDouble("elevator position", elevator::getEncoderPosition);
-
 		/* 
 		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
 			.withSize(1, 1)
@@ -243,7 +217,7 @@ public class RobotContainer {
 			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(() -> 1));
+			.andThen(manipulator.runUntilCollected(1));
 		}
 
 	/**
@@ -257,7 +231,7 @@ public class RobotContainer {
 			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
-			.andThen(manipulator.runUntilCollected(() -> 1));
+			.andThen(manipulator.runUntilCollected(1));
 	}
 
 	/**

src/main/java/frc/robot/constants/ClimberPivotConstants.java

@@ -3,7 +3,7 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ClimberPivotConstants {
-    public static final int kPivotMotorID = 10;
+    public static final int kPivotMotorID = 0;
 
     public static final int kClimberLimitSwitchID = 0;
 

src/main/java/frc/robot/constants/ClimberRollersConstants.java

@@ -3,7 +3,7 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ClimberRollersConstants {
-    public static final int kRollerMotorID = 9;
+    public static final int kRollerMotorID = 0;
 
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -22,10 +22,10 @@ public class DrivetrainConstants {
     public static final double kWheelBase = Units.inchesToMeters(24.5);
 
     // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
-    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kFrontRightChassisAngularOffset = 0;
-    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI;
-    public static final double kBackRightChassisAngularOffset = 0;
+    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
 
     // 1, 7, 10 is the default for these three values
     public static final double kSysIDDrivingRampRate = 1;
@@ -38,15 +38,15 @@ public class DrivetrainConstants {
     public static final double kSysIDTurningTimeout = 10;
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 0;
+    public static final int kFrontLeftDrivingCanId = 11;
-    public static final int kRearLeftDrivingCanId = 2;
+    public static final int kRearLeftDrivingCanId = 13;
-    public static final int kFrontRightDrivingCanId = 1;
+    public static final int kFrontRightDrivingCanId = 15;
-    public static final int kRearRightDrivingCanId = 3;
+    public static final int kRearRightDrivingCanId = 17;
 
-    public static final int kFrontLeftTurningCanId = 2;
+    public static final int kFrontLeftTurningCanId = 10;
-    public static final int kRearLeftTurningCanId = 4;
+    public static final int kRearLeftTurningCanId = 12;
-    public static final int kFrontRightTurningCanId = 7;
+    public static final int kFrontRightTurningCanId = 14;
-    public static final int kRearRightTurningCanId = 5;
+    public static final int kRearRightTurningCanId = 16;
 
     public static final boolean kGyroReversed = true;
 

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -11,17 +11,17 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
-    public static final int kElevatorMotor1ID = 8;
+    public static final int kElevatorMotor1ID = 0;
-    public static final int kElevatorMotor2ID = 6;
+    public static final int kElevatorMotor2ID = 0;
 
     public static final int kBottomLimitSwitchID = 0;
 
     // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
-    public static final double kEncoderConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
+    public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0.2;
+    public static final double kPositionControllerP = 0;
     public static final double kPositionControllerI = 0;
     public static final double kPositionControllerD = 0;
 
@@ -73,10 +73,10 @@ public class ElevatorConstants {
     static {
         motorConfig
             .smartCurrentLimit(kCurrentLimit)
-            .idleMode(kIdleMode)
+            .idleMode(kIdleMode);
-            .inverted(false);
         motorConfig.encoder
-            .positionConversionFactor(kEncoderConversionFactor);   
+            .positionConversionFactor(kEncoderConversionFactor)
+            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
         motorConfig.closedLoop
             .p(kPositionControllerP)
             .i(kPositionControllerI)

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -3,8 +3,9 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
-    public static final int kManipulatorMotorID = 12;
+    public static final int kManipulatorMotorID = 0;
-    public static final int kCoralBeamBreakID = 2;
+    public static final int kCoralBeamBreakID = 0;
+    public static final int kAlgaeBeamBreakID = 0;
 
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -13,11 +13,11 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ManipulatorPivotConstants {
-    public static final int kArmMotorID = 1;
+    public static final int kArmMotorID = 0;
 
     public static final int kMotorCurrentMax = 40;
 
-    public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0;
+    public static final double kPivotConversion = 12/60 * 20/60 * 12/28;
 
     public static final double kPivotMaxVelocity = 0;
 

src/main/java/frc/robot/subsystems/ClimberPivot.java

@@ -16,7 +16,7 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
-    //private DigitalInput cageLimitSwitch;
+    private DigitalInput cageLimitSwitch;
 
     public ClimberPivot() {
         pivotMotor = new SparkMax(
@@ -32,7 +32,7 @@ public class ClimberPivot extends SubsystemBase {
 
         neoEncoder = pivotMotor.getEncoder();
 
-        //cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
+        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
     }
 
     public Command runPivot(double speed) {
@@ -59,9 +59,9 @@ public class ClimberPivot extends SubsystemBase {
      * 
      * @return Is the cage in the climber
      */
-   // public boolean getCageLimitSwitch() {
+    public boolean getCageLimitSwitch() {
-    //    return cageLimitSwitch.get();
+        return cageLimitSwitch.get();
-    //}
+    }
 
     public double getEncoderPosition() {
         return neoEncoder.getPosition();

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -228,7 +228,7 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        ahrs.reset();
+        ahrs.reset();;
     }
 
     public double getGyroValue() {

src/main/java/frc/robot/subsystems/Elevator.java

@@ -1,6 +1,5 @@
 package frc.robot.subsystems;
 
-import java.util.ResourceBundle.Control;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
@@ -8,7 +7,6 @@ import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -80,7 +78,7 @@ public class Elevator extends SubsystemBase {
 
     @Override
     public void periodic() {
-        if (!getBottomLimitSwitch()) {
+        if (getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
     }
@@ -141,7 +139,8 @@ public class Elevator extends SubsystemBase {
     
     public Command manualControl(DoubleSupplier speed){
         return run(() -> {
-            elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
+            
+            elevatorMotor1.setVoltage(feedForward.calculate(0) + (speed.getAsDouble()/12));
 
         });
     }
@@ -167,7 +166,7 @@ public class Elevator extends SubsystemBase {
                 SparkBase.ControlType.kMAXMotionPositionControl,
                 ClosedLoopSlot.kSlot0,
                 feedForward.calculate(0)
-            );
+                );
 
         });
 /* 
@@ -212,7 +211,6 @@ public class Elevator extends SubsystemBase {
         return encoder.getPosition();
     }
 
-
     /**
      * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
      * 

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -42,8 +42,7 @@ public class Manipulator extends SubsystemBase {
      */
     public Command defaultCommand() {
         return run(() -> {
-            //runUntilCollected(0.1);
+            runUntilCollected(0.1);
-            manipulatorMotor.set(0);
             });
     }
 
@@ -69,12 +68,12 @@ public class Manipulator extends SubsystemBase {
      * @param coral Is the object a coral? (True = Coral, False = Algae)
      * @return Returns a command that sets the speed of the motor
      */
-    public Command runUntilCollected(DoubleSupplier speed) {
+    public Command runUntilCollected(double speed) {
         return run(() -> {
             manipulatorMotor.set(
-                speed.getAsDouble()
+                speed * -1
             );
-        }).until(() -> !coralBeamBreak.get());
+        }).until(() -> coralBeamBreak.get());
     }
 
     public boolean getCoralBeamBreak() {