First wave of basic fixes against real robot

src/main/java/competition/electrical_contract/CompetitionContract.java

@@ -17,7 +17,7 @@ public CompetitionContract() {
 
     @Override
     public boolean isShooterReady() {
-        return false;
+        return true;
     }
 
     @Override
@@ -103,29 +103,29 @@ public XYPair getSwerveModuleOffsets(SwerveInstance swerveInstance) {
     }
 
     public DeviceInfo getCollectorMotor() {
-        return new DeviceInfo("CollectorMotor", 25, true);
+        return new DeviceInfo("CollectorMotor", 37, false);
     }
 
     @Override
     public DeviceInfo getShooterMotorLeader() {
-        return new DeviceInfo("ShooterLeader", 50, false);
+        return new DeviceInfo("ShooterLeader", 23, false);
     }
 
     @Override
     public DeviceInfo getShooterMotorFollower() {
-        return new DeviceInfo("ShooterFollower", 49, false);
+        return new DeviceInfo("ShooterFollower", 36, false);
     }
 
     @Override
     public boolean isCollectorReady() {
-        return false;
+        return true;
     }
 
     public boolean isScoocherReady() {
-        return false;
+        return true;
     }
     public DeviceInfo getScoocherMotor(){
-        return new DeviceInfo("ScoocherMotor", 14);
+        return new DeviceInfo("ScoocherMotor", 33);
     }
 
     public DeviceInfo getCollectorSolenoid() {

src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java

@@ -12,6 +12,7 @@
 import competition.subsystems.schoocher.commands.StopScoocherCommand;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
 import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand;
+import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand;
 import xbot.common.injection.swerve.FrontLeftDrive;
 import xbot.common.injection.swerve.FrontRightDrive;
 import xbot.common.injection.swerve.RearLeftDrive;
@@ -48,7 +49,11 @@ public void setUpCollectorSubsystem(CollectorSubsystem collectorSubsystem, StopC
     }
 
     @Inject
-    public void setupShooterWheelSubsystem(ShooterWheelSubsystem shooterWheelSubsystem, ShooterWheelMaintainerCommand command) {
+    public void setupShooterWheelSubsystem(ShooterWheelSubsystem shooterWheelSubsystem,
+                                           ShooterWheelMaintainerCommand command,
+                                           WarmUpShooterRPMCommand setToZero) {
         shooterWheelSubsystem.setDefaultCommand(command);
+        setToZero.setTargetRpm(0.0);
+        shooterWheelSubsystem.getSetpointLock().setDefaultCommand(setToZero);
     }
 }

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -136,14 +136,17 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
             armMotorRight = sparkMaxFactory.createWithoutProperties(
                     contract.getArmMotorRight(), this.getPrefix(), "ArmMotorRight");
 
+            armMotorLeft.enableVoltageCompensation(12);
+            armMotorRight.enableVoltageCompensation(12);
+
             // Get through-bore encoders
             var armWithEncoder = contract.getArmEncoderIsOnLeftMotor() ? armMotorLeft : armMotorRight;
             armAbsoluteEncoder = armWithEncoder.getAbsoluteEncoder(
                     this.getPrefix() + "ArmEncoder",
                     contract.getArmEncoderInverted());
 
-            armMotorLeft.setSmartCurrentLimit(20);
-            armMotorRight.setSmartCurrentLimit(20);
+            armMotorLeft.setSmartCurrentLimit(40);
+            armMotorRight.setSmartCurrentLimit(40);
 
             // Enable hardware limits
             armMotorLeft.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
@@ -208,7 +211,7 @@ public void setPowerToLeftAndRightArms(double leftPower, double rightPower) {
             // If we have to make any changes to power, do so by a factor proportional to the maximum
             // allowed desync in mm. At 50% of the desync, it would restrict power by 50%.
 
-            double potentialReductionFactor = Math.min(1, Math.abs(distanceLeftAhead) / maximumArmDesyncInMm.get());
+            double potentialReductionFactor = Math.max(0, 1 - Math.abs(distanceLeftAhead) / maximumArmDesyncInMm.get());
             aKitLog.record("PotentialReductionFactor", potentialReductionFactor);
 
             // If left arm is ahead
@@ -261,7 +264,6 @@ else if (distanceLeftAhead < 0) {
             rightPower = MathUtils.constrainDouble(rightPower, speedLimitForNotCalibrated.get(), 0);
         }
 
-
         // If calibrated, but near limits, slow the system down a bit so we
         // don't slam into the hard limits.
         if (hasCalibratedLeft && hasCalibratedRight)
@@ -273,7 +275,7 @@ else if (distanceLeftAhead < 0) {
                     rightPower,
                     getRightArmPosition());
         }
-  
+
         // If we are actually at our hard limits, stop the motors
         leftPower = constrainPowerIfAtLimit(armMotorLeft, leftPower);
         rightPower = constrainPowerIfAtLimit(armMotorRight, rightPower);

src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java

@@ -56,7 +56,10 @@ protected double getErrorMagnitude() {
 
     @Override
     protected Double getHumanInput() {
-        return MathUtils.deadband(oi.operatorGamepad.getLeftVector().y, oi.getOperatorGamepadTypicalDeadband());
+        return MathUtils.deadband(
+                oi.operatorGamepad.getLeftVector().y,
+                oi.getOperatorGamepadTypicalDeadband(),
+                (x) -> x);
     }
 
     @Override

src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java

@@ -26,7 +26,7 @@ public ScoocherSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sp
             this.scoocherMotor = sparkMaxFactory.createWithoutProperties(contract.getScoocherMotor(), getPrefix(), "Scoocher");
         }
         pf.setPrefix(this);
-        sendingPower = pf.createPersistentProperty("sendingPower", 0.1);
+        sendingPower = pf.createPersistentProperty("sendingPower", 0.75);
     }
 
     private void setPower(double power) {

src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java

@@ -9,6 +9,7 @@
 import xbot.common.command.BaseSetpointSubsystem;
 import competition.electrical_contract.ElectricalContract;
 import xbot.common.controls.actuators.XCANSparkMax;
+import xbot.common.controls.actuators.XCANSparkMaxPIDProperties;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
 
@@ -82,23 +83,25 @@ public ShooterWheelSubsystem(XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, P
         // THIS IS HOW MUCH RPM WE CAN TOLERATE (needs testing and is UNIVERSAL)
         acceptableToleranceRPM = pf.createPersistentProperty("AcceptableToleranceRPM", 200);
 
-
-        // MOTOR RELATED, COULD BE USED LATER
-//        XCANSparkMaxPIDProperties wheelDefaultProps = new XCANSparkMaxPIDProperties();
-//        wheelDefaultProps.p = 0.00008;
-//        wheelDefaultProps.i = 0;
-//        wheelDefaultProps.d = 0;
-//        wheelDefaultProps.feedForward = 0.000185;
-//        wheelDefaultProps.iZone = 200;
-//        wheelDefaultProps.maxOutput = 1;
-//        wheelDefaultProps.minOutput = -1;
+        XCANSparkMaxPIDProperties defaultShooterPidProperties = new XCANSparkMaxPIDProperties(
+                0.0007,
+                0.0,
+                0.0,
+                0.0,
+                0.00019,
+                1,
+                -1
+        );
 
         if (contract.isShooterReady()) {
             this.leader = sparkMaxFactory.create(contract.getShooterMotorLeader(), this.getPrefix(),
-                    "ShooterMaster", null);
-            this.follower = sparkMaxFactory.create(contract.getShooterMotorFollower(), this.getPrefix(),
-                    "ShooterFollower", null);
-            this.follower.follow(this.leader, true);
+                    "ShooterMaster", "ShooterWheel", defaultShooterPidProperties);
+            this.follower = sparkMaxFactory.createWithoutProperties(contract.getShooterMotorFollower(), this.getPrefix(),
+                    "ShooterFollower");
+            this.follower.follow(this.leader, false);
+
+            this.leader.setP(defaultShooterPidProperties.p());
+            this.leader.setFF(defaultShooterPidProperties.feedForward());
         }
     }
 
@@ -204,6 +207,9 @@ public void configurePID() {
         }
     }
     public void periodic() {
+        leader.periodic();
+        follower.periodic();
+
         aKitLog.record("TargetRPM", getTargetValue());
         aKitLog.record("CurrentRPM", getCurrentValue());
         aKitLog.record("TrimRPM", getTrimRPM());