odometry update is just trolling

src/main/deploy/pathplanner/center_mobility.path

@@ -2,13 +2,13 @@
   "waypoints": [
     {
       "anchorPoint": {
-        "x": 1.0,
-        "y": 3.0
+        "x": 1.8058163911466711,
+        "y": 0.4585108250077607
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0,
-        "y": 3.0
+        "x": 2.8058163911466787,
+        "y": 0.4585108250077607
       },
       "holonomicAngle": 0,
       "isReversal": false,
@@ -24,19 +24,19 @@
     },
     {
       "anchorPoint": {
-        "x": 3.2257993235545075,
-        "y": 2.9976040783512325
+        "x": 7.0900275255411636,
+        "y": 0.9205009278074111
       },
       "prevControl": {
-        "x": 2.43846492164065,
-        "y": 3.0023737159346475
+        "x": 6.302693123627306,
+        "y": 0.9252705653908261
       },
       "nextControl": {
-        "x": 4.225780974640867,
-        "y": 2.9915462329755536
+        "x": 6.302693123627306,
+        "y": 0.9252705653908261
       },
       "holonomicAngle": 0,
-      "isReversal": false,
+      "isReversal": true,
       "velOverride": 1.0,
       "isLocked": false,
       "isStopPoint": false,
@@ -49,12 +49,37 @@
     },
     {
       "anchorPoint": {
-        "x": 6.162371757772435,
-        "y": 3.025877703201374
+        "x": 1.8230968041797195,
+        "y": 1.068776149705077
+      },
+      "prevControl": {
+        "x": 2.268778005912175,
+        "y": 1.068776149705077
+      },
+      "nextControl": {
+        "x": 2.268778005912175,
+        "y": 1.068776149705077
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.8415394018288036,
+        "y": 2.0653572852284596
       },
       "prevControl": {
-        "x": 6.015742280723526,
-        "y": 3.175457471538993
+        "x": 2.569189420969124,
+        "y": 2.0653572852284596
       },
       "nextControl": null,
       "holonomicAngle": 0,
@@ -72,7 +97,7 @@
   ],
   "maxVelocity": 6.0,
   "maxAcceleration": 3.0,
-  "isReversed": null,
+  "isReversed": false,
   "markers": [
     {
       "position": 0.5117897727272727,

src/main/java/org/usfirst/frc4904/robot/Robot.java

@@ -6,25 +6,20 @@
 /*----------------------------------------------------------------------------*/
 package org.usfirst.frc4904.robot;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain;
 import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator;
 import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem;
 import org.usfirst.frc4904.robot.seenoevil.RobotContainer2;
-import org.usfirst.frc4904.robot.subsystems.arm.ArmPivotSubsystem;
 import org.usfirst.frc4904.standard.CommandRobotBase;
 import org.usfirst.frc4904.standard.humaninput.Driver;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
 
-import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 import static org.usfirst.frc4904.robot.Utils.nameCommand;
@@ -33,10 +28,14 @@
 public class Robot extends CommandRobotBase {
     private final RobotMap map = new RobotMap();
     private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.frontLeftWheelTalon, RobotMap.Component.backLeftWheelTalon, RobotMap.Component.frontRightWheelTalon, RobotMap.Component.backRightWheelTalon, RobotMap.Component.navx);
-
+    private final NathanGain nathangain = new NathanGain();
     private final Driver driver = new NathanGain();
     private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator();
 
+    protected double scaleGain(double input, double gain, double exp) {
+		return Math.pow(Math.abs(input), exp) * gain * Math.signum(input);
+	}
+
     @Override
     public void initialize() {
     }
@@ -47,20 +46,21 @@ public void teleopInitialize() {
         if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); 
         if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); 
         if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); 
-        RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput();
-        RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral();
+        RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.brake();
 
-        RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setBrakeOnNeutral();
-        RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput();
+        RobotMap.Component.arm.armExtensionSubsystem.motor.brake();
+
+        driver.bindCommands();
+        operator.bindCommands();
 
         /***********************
          * HAZMAT BLOCK START
         *************************/
         // SATURDAY MORNING TEST - can you run drive train in queueline
 
-        donttouchme.m_robotDrive.m_leftMotors = null;
-        donttouchme.m_robotDrive.m_rightMotors = null;
-        donttouchme.m_robotDrive.m_drive = null;
+        // donttouchme.m_robotDrive.m_leftMotors = null;
+        // donttouchme.m_robotDrive.m_rightMotors = null;
+        // donttouchme.m_robotDrive.m_drive = null;
         DriveSubsystem.skuffedaf_teleop_initialized = true;
 
 
@@ -91,37 +91,17 @@ public void teleopInitialize() {
         //     RobotMap.Component.arm.c_posReturnToHomeUp(NathanGain.isFlippy)
         // ));
 
-        final DoubleSupplier pivot_getter = () -> RobotMap.HumanInput.Operator.joystick.getAxis(1) * 50;  
+        final DoubleSupplier pivot_getter = () -> scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(1),60, 1);  
         (new Trigger(() -> pivot_getter.getAsDouble() != 0)).onTrue(
             nameCommand("arm - teleop - armPivot operator override",
                 RobotMap.Component.arm.armPivotSubsystem.c_controlAngularVelocity(pivot_getter::getAsDouble)
             )
         );
 
-        RobotMap.HumanInput.Operator.joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3));
-        RobotMap.HumanInput.Operator.joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0));
-
-        RobotMap.HumanInput.Operator.joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3));
-        RobotMap.HumanInput.Operator.joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0));
-
-
         // Intake
 		// FIXME: use nameCommand to make it cleaner with expresions (no varibales) 
-        var cmdnull = RobotMap.Component.intake.c_holdVoltage(0);
+        // var cmdnull = RobotMap.Component.intake.c_holdVoltage(0);
 
-        var cmd2 = RobotMap.Component.intake.c_holdVoltage(-8);
-		cmd2.setName("Intake - manual intake activation");
-        var cmdhold = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1));
-
-		cmdnull.setName("Intake - deactivated");
-		RobotMap.HumanInput.Operator.joystick.button2.onTrue(cmd2);
-        RobotMap.HumanInput.Operator.joystick.button2.onFalse(cmdhold);
-
-		// Outtake
-		var cmd1 = RobotMap.Component.intake.c_holdVoltage(3);
-		cmd1.setName("Intake - manual outtake activation");
-		RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1);
-        RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull);
     }
 
     @Override
@@ -136,12 +116,17 @@ public void teleopExecute() {
         // SmartDashboard.putNumber("Driver out", driver.getTurnSpeed());
 
 
-        SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy);
+        // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy);
+        // if (RobotMap.HumanInput.Operator.joystick.getPOV() != 0) {
         SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle());
         SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength());
+        SmartDashboard.putNumber("zeroing", RobotMap.Component.arm.armExtensionSubsystem.motor.getSensorPositionRotations());
         SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees());
+        //}
+        // SmartDashboard.putNumber("Falcon temp",  RobotContainer2.Component.leftATalonFX.getTemperature());
 
-        SmartDashboard.putNumber("Falcon temp",  RobotContainer2.Component.leftATalonFX.getTemperature());
+        SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem);
+        SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem);
 
 
     }
@@ -172,7 +157,9 @@ public void autonomousInitialize() {
 
         // SATURDAY MORNING TEST: is the cube shooter auton gonna work
         // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts);
-        var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward"));
+        // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts);
+
+        var commnand = donttouchme.simpleAuton(); //for calgames; just place a cone and leave the zone
         commnand.schedule();
     }
 
@@ -181,7 +168,7 @@ public void autonomousExecute() {
         // TODO remove logging
 
 
-        SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy);
+        // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy);
         SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString());
         SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX());
         SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY());
@@ -191,8 +178,12 @@ public void autonomousExecute() {
         SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength());
         SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees());
 
-        SmartDashboard.putNumber("Falcon temp",  RobotContainer2.Component.leftATalonFX.getTemperature());
+        // SmartDashboard.putNumber("Falcon temp",  RobotContainer2.Component.leftATalonFX.getTemperature());
 
+        // SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem);
+        // SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem);
+        // SmartDashboard.putData(RobotMap.Component.arm);
+        // SmartDashboard.putData(donttouchme.m_robotDrive);
     }
 
     @Override
@@ -232,18 +223,17 @@ public void testInitialize() {
     @Override
     public void testExecute() {
         RobotMap.Component.arm.armExtensionSubsystem.initializeEncoderPositions(0);
-
-
 
     }
 
     @Override
     public void alwaysExecute() {
-        // SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees());
-        // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle());
+        SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees());
+        SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength());
+        SmartDashboard.putNumber("intake speed", RobotMap.HumanInput.Operator.joystick.getAxis(3));
 
 
-
+        // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle());
     }
 
 }

src/main/java/org/usfirst/frc4904/robot/RobotMap.java

@@ -28,6 +28,7 @@
 import org.usfirst.frc4904.standard.subsystems.chassis.WestCoastDrive;
 import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem;
 import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotMap {
@@ -41,6 +42,8 @@ public static class Port {
         public static class HumanInput {
             public static final int joystick = 0;
             public static final int xboxController = 1;
+            public static final int throttleJoystick = 2;
+            public static final int turnJoystick = 3;
         }
 
         // // blinky constants
@@ -87,8 +90,8 @@ public static class Metrics {
         // // 2023-robot constants
         public static class Chassis {
             public static final double GEAR_RATIO = 496/45; // https://www.desmos.com/calculator/llz7giggcf
-            public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5 + 1/8);
-            public static final double TRACK_WIDTH_METERS = .533; // +/- 0.5 inches
+            public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5.12);
+            public static final double TRACK_WIDTH_METERS = .8713; // +/- 0.5 inches
             public static final double CHASSIS_LENGTH = Units.inchesToMeters(37); // +/- 0.5 inches
 
         }
@@ -155,6 +158,8 @@ public static class Input {
     public static class HumanInput {
         public static class Driver {
             public static CustomCommandXbox xbox;
+            public static CustomCommandJoystick throttleJoystick;
+            public static CustomCommandJoystick turnJoystick;
         }
 
         public static class Operator {
@@ -166,7 +171,9 @@ public RobotMap() {
         Component.navx = new AHRS(SerialPort.Port.kMXP);
 
         HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.1);
-		HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.05);
+		HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.1);
+        HumanInput.Driver.throttleJoystick = new CustomCommandJoystick(Port.HumanInput.throttleJoystick, 0.1);
+        HumanInput.Driver.turnJoystick = new CustomCommandJoystick(Port.HumanInput.turnJoystick, 0.1);
         // // UDP things
         // try {
         //     Component.robotUDP = new RobotUDP(Port.Network.LOCAL_SOCKET_ADDRESS, Port.Network.LOCALIZATION_ADDRESS);
@@ -185,6 +192,11 @@ public RobotMap() {
         Component.backLeftWheelTalon   = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_A, InvertType.None);
         Component.frontLeftWheelTalon  = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B, InvertType.None);
 
+        // Component.backRightWheelTalon.setSafetyEnabled(false);
+        // Component.frontRightWheelTalon.setSafetyEnabled(false);
+        // Component.backLeftWheelTalon.setSafetyEnabled(false);
+        // Component.frontLeftWheelTalon.setSafetyEnabled(false);
+
         TalonMotorSubsystem leftDriveMotors  = new TalonMotorSubsystem("left drive motors",  NeutralMode.Brake, 0, Component.frontLeftWheelTalon, Component.backLeftWheelTalon);
         TalonMotorSubsystem rightDriveMotors = new TalonMotorSubsystem("right drive motors", NeutralMode.Brake, 0, Component.frontRightWheelTalon, Component.backRightWheelTalon);
         Component.chassis = new WestCoastDrive(
@@ -205,7 +217,7 @@ public RobotMap() {
         TalonMotorSubsystem armRotationMotors = new TalonMotorSubsystem("Arm Pivot Subsystem", NeutralMode.Brake, 0, leftPivotMotor, rightPivotMotor);
         ArmExtensionSubsystem armExtensionSubsystem = new ArmExtensionSubsystem(
             new TalonMotorSubsystem("Arm Extension Subsystem", NeutralMode.Brake, 0, armExtensionMotor),
-            () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations())
+            () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations() * 0.911 - 6.3)
         );
         // Autonomous.autonCommand = Component.chassis.c_buildPathPlannerAuto(
         //     PID.Drive.kS, PID.Drive.kV, PID.Drive.kA,