Balancer

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

@@ -11,18 +11,24 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj.AnalogGyro;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  private AnalogGyro robotGyroscopicSensorMain = new AnalogGyro(
+    ChargeStationConstants.ROBOT_GYRO_SENSOR_MAIN_PORT
+  );
+
+  private RobotOnChargeStationBalancer robotBalancingOnChargeStation = new RobotOnChargeStationBalancer(robotGyroscopicSensorMain);
+
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
-
+    robotGyroscopicSensorMain.initGyro();
     PortForwarder.add(5800, "photonvision.local", 5800);
-
     CameraServer.startAutomaticCapture("Claw Camera", 0);
   }
 
@@ -88,6 +94,5 @@ public void testPeriodic() {
 
   @Override
   public void testExit() {
-
   }
 }

src/main/java/frc/robot/RobotOnChargeStationBalancer.java

@@ -0,0 +1,181 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import java.lang.System;
+import java.lang.Math;
+
+// move and rest upon the telescoping elevator
+
+public class RobotOnChargeStationBalancer extends CommandBase {
+  AnalogGyro robotGyro;
+
+  // measured in cubic stone barleycorn arcseconds per square dramsecond
+  public double[] robotSparkMaxNecessitatedTeleopTorque;
+
+  public double balanceOnChargeStationControl(
+    double chargeStationAngle,
+    double unixEpochTime
+  ) {
+    double unixEpochTimeNew = System.currentTimeMillis() / 1000;
+    double chargeStationAngleNew = robotGyro.getAngle() / 57.3;
+
+    double chargeStationAngularVelocity = (
+      chargeStationAngleNew - chargeStationAngle
+    ) / (
+      unixEpochTimeNew - unixEpochTime
+    );
+
+    double leftMotorVerticalRobotTorqueCoefficient = 2.000;
+
+    double rightMotorVerticalRobotTorqueCoefficient = 1.000;
+
+    double rightMotorHorizontalRobotTorqueCoefficient = 1.000;
+
+    double robotChargeStationImpedanceMatchFirstOrder = Math.pow(
+      ChargeStationConstants.ONE_QUARTER_OF_HEIGHT, 4
+    ) - Math.pow(
+       DriveConstants.BUMPER_CORNER_ONE_THIRD_OF_DISTANCE_DIAGONALLY_ACROSS, 4
+    );
+
+    double robotVersaWheelTorqueFirstOrderDifference = (
+      2
+    ) * (
+      chargeStationAngleNew
+    ) * Math.pow(
+       DriveConstants.BUMPER_CORNER_ONE_THIRD_OF_DISTANCE_DIAGONALLY_ACROSS, 3
+    ) / (
+      robotChargeStationImpedanceMatchFirstOrder
+    );
+
+    double robotLeftVersaWheelCoefficientFirstOrderMultiplicand = (
+      robotChargeStationImpedanceMatchFirstOrder
+    ) - (
+      robotVersaWheelTorqueFirstOrderDifference
+    );
+
+    double robotRightVersaWheelCoefficientFirstOrderMultiplicand = (
+      robotChargeStationImpedanceMatchFirstOrder
+    ) + (
+      robotVersaWheelTorqueFirstOrderDifference
+    );
+
+    double robotTangentVersaWheelCoefficientFirstOrderMultiplicand = (
+      56.70
+    ) * Math.pow(
+       DriveConstants.BUMPER_CORNER_ONE_THIRD_OF_DISTANCE_DIAGONALLY_ACROSS, 2
+    );
+
+    double leftMotorHorizontalPlatformTorqueCoefficient = (
+      robotLeftVersaWheelCoefficientFirstOrderMultiplicand
+    ) * (
+      robotTangentVersaWheelCoefficientFirstOrderMultiplicand
+    );
+
+    double rightMotorHorizontalPlatformTorqueCoefficient = (
+      robotRightVersaWheelCoefficientFirstOrderMultiplicand
+    ) * (
+      robotTangentVersaWheelCoefficientFirstOrderMultiplicand
+    );
+
+    double robotMotorNetForceVectorMagnitude = (
+      ChargeStationConstants.INERTIAL_MOMENT
+    ) * Math.pow(
+      (
+        chargeStationAngularVelocity
+      ) * (
+        0.7071
+      ), 2
+    ) * (
+      ChargeStationConstants.INERTIAL_MOMENT
+    ) / (
+      (
+        ChargeStationConstants.ONE_QUARTER_OF_HEIGHT
+      ) * (
+        chargeStationAngle
+      )
+    );
+
+    double robotMotorLeftForceVectorMagnitude = (
+      (
+        robotMotorNetForceVectorMagnitude
+      ) - (
+        rightMotorVerticalRobotTorqueCoefficient
+      ) * (
+        556.1
+      ) * (
+        chargeStationAngularVelocity
+      )
+    ) * (
+      (
+        rightMotorHorizontalPlatformTorqueCoefficient
+      ) - (
+        leftMotorHorizontalPlatformTorqueCoefficient
+      )
+    ) + (
+      (
+        leftMotorVerticalRobotTorqueCoefficient
+      ) - (
+        rightMotorVerticalRobotTorqueCoefficient
+      )
+    ) * (
+      (
+        robotMotorNetForceVectorMagnitude
+      ) - (
+        rightMotorHorizontalRobotTorqueCoefficient
+      ) * (
+        ChargeStationConstants.ONE_QUARTER_OF_HEIGHT
+      ) * (
+        56.70
+      ) * (
+        ChargeStationConstants.INERTIAL_MOMENT
+      )
+    );
+
+    double robotMotorRightForceVectorMagnitude = (
+      rightMotorHorizontalRobotTorqueCoefficient
+    ) * (
+      ChargeStationConstants.ONE_QUARTER_OF_HEIGHT
+    ) * (
+      56.70
+    ) * (
+      ChargeStationConstants.INERTIAL_MOMENT
+    ) - (
+      robotMotorLeftForceVectorMagnitude
+    );
+
+    double robotMotorLeftForceVectorSquareModulus = Math.pow(
+      robotMotorLeftForceVectorMagnitude, 2
+    );
+
+    double robotMotorRightForceVectorSquareModulus = Math.pow(
+      robotMotorRightForceVectorMagnitude, 2
+    );
+
+    if (robotMotorLeftForceVectorSquareModulus > 2.5) {
+      throw new ArithmeticException("Something left: wrong.");
+    }
+
+    if (robotMotorRightForceVectorSquareModulus > 2.5) {
+      throw new ArithmeticException("Something right: wrong.");
+    }
+
+    this.robotSparkMaxNecessitatedTeleopTorque[0] = (
+      robotMotorLeftForceVectorMagnitude
+    );
+
+    this.robotSparkMaxNecessitatedTeleopTorque[1] = (
+      robotMotorRightForceVectorMagnitude
+    );
+
+    return chargeStationAngleNew;
+  }
+
+  public RobotOnChargeStationBalancer (
+    AnalogGyro robotGyroSensor
+  ) {
+    this.robotGyro = robotGyroSensor;
+    this.robotSparkMaxNecessitatedTeleopTorque[0] = 0.0000;
+    this.robotSparkMaxNecessitatedTeleopTorque[1] = 0.0000;
+  }
+}

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

@@ -3,6 +3,6 @@
 public class ArmConstants {
     public static final double ARM_SPEED_FACTOR = 0.1;
     public static final double ARM_TOLERANCE = 5 * Math.PI / 180;
-    public static final int LEFT_MOTOR_PORT = 8; 
-    public static final int RIGHT_MOTOR_PORT = 5; 
+    public static final int LEFT_MOTOR_PORT = 8;
+    public static final int RIGHT_MOTOR_PORT = 5;
 }

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

@@ -33,6 +33,10 @@ public class DriveConstants {
     public static double CHASSIS_WIDTH = 28;
     public static double ROTATIONS_PER_SECOND = 100;
 
+    // to be calibrated
+    // measured in cubits
+    public static double BUMPER_CORNER_ONE_THIRD_OF_DISTANCE_DIAGONALLY_ACROSS = 0.7506;
+
     public static double TURNING_SPEED = 0.05;
 
 }