Chezy Day 1.

src/com/_604robotics/robot2020/Robot2020.java

@@ -37,6 +37,8 @@ private Robot2020() {
 
   public final Shooter shooter = addModule(new Shooter());
 
+  public final Climber climber = addModule(new Climber());
+
   public final Swerve drive = addModule(new Swerve());
 
   public final Limelight limelight = addModule(new Limelight(

src/com/_604robotics/robot2020/constants/Calibration.java

@@ -18,8 +18,7 @@ public class Calibration {
 
   /* Climber Calibration */
   public static final class Climber {
-    public static final double CLIMB_SPEED = -0.25;
-    public static final double RETRACT_SPEED = 0.25;
+    public static final double CLIMB_SPEED = 0.5;
   }
 
   /* Tower Calibration */
@@ -32,7 +31,7 @@ public static final class Tower {
   /* Revolver Calibration */
   public static final class Revolver {
     // public static final double EMPTY_SPEED = 0.75;
-    public static final double EMPTY_SPEED = 0.4;
+    public static final double EMPTY_SPEED = 0.6;
     public static final double INTAKE_SPEED = 0.5;
     public static final double REVERSE_SPEED = -0.5;
   }

src/com/_604robotics/robot2020/constants/Ports.java

@@ -19,7 +19,7 @@ public class Ports {
   public static final int MODULE_3_ABS_ENCODER = 3;
 
   /* Climber */
-  public static final int CLIMBER_MOTOR = 4;
+  public static final int CLIMBER_MOTOR = 16;
 
   /* Shifter */
   public static final int SHIFTER_A = 4;

src/com/_604robotics/robot2020/modes/AutonomousMode.java

@@ -70,6 +70,10 @@ public AutonomousMode(Robot2020 robot) {
   public void begin() {
     System.out.println(autonMode.get());
 
+    reader.loadChosenFile();
+
+    robot.drive.zeroGyroOffset();
+
     switch (autonMode.get()) {
       case MANUAL:
         selectedModeMacro = robot.teleopMode;

src/com/_604robotics/robot2020/modes/TeleopMode.java

@@ -53,6 +53,8 @@ public class TeleopMode extends Coordinator {
   private boolean autoCentering = false;
   private boolean aligned = false;
 
+  private boolean climbMode = false;
+
   private final Logger test = new Logger("Teleop");
 
   public TeleopMode(com._604robotics.robot2020.Robot2020 robot) {
@@ -272,17 +274,35 @@ public void run() {
       }
 
       if (driverStart) {
-        robot.drive.zeroGyro();
+        climbMode = true;
+
+        if (driverBack) {
+          robot.climber.extend.activate();
+        } else if (driverRightBumper) {
+          robot.climber.climb.activate();
+        } else if (driverLeftBumper) {
+          robot.climber.retract.activate();
+        } else {
+          robot.climber.idle.activate();
+        }
+
+      } else {
+        climbMode = false;
+        robot.climber.idle.activate();
       }
 
-      if (driverY || driverA || driverB || driverX) {
+      if (driverY || driverA || driverB) {
         currentDrive = CurrentDrive.SNAPPING;
       }
 
       if (Math.abs(rightX) > 0.1) {
         currentDrive = CurrentDrive.OPENLOOP;
       }
 
+      if (driverX) {
+        robot.drive.zeroGyroOffset();
+      }
+
       // // Get Dashboard option for drive
       // switch (robot.drive.driveMode.get()) {
       //   case OFF:
@@ -337,8 +357,6 @@ public void run() {
             snapTo(CardinalDirections.SOUTH);
           } else if (driverB) {
             snapTo(CardinalDirections.EAST);
-          } else if (driverX) {
-            snapTo(CardinalDirections.WEST);
           }
 
           autoAngle.activate();
@@ -422,7 +440,7 @@ public void run() {
         revolve.activate();
         roller.activate();
         antiJam.activate();
-      } else if (driverLeftBumper) {
+      } else if (driverLeftBumper && !climbMode) {
         reverseIntake.activate();
         reverseRevolver.activate();
         reverseRoller.activate();
@@ -481,7 +499,7 @@ public void run() {
         stop.activate();
       }
 
-      if (driverRightBumper) {
+      if (driverRightBumper && !climbMode) {
         emptyTower.activate();
         emptyRevolver.activate();
         roller.activate();

src/com/_604robotics/robot2020/modules/Climber.java

@@ -0,0 +1,93 @@
+package com._604robotics.robot2020.modules;
+
+import com._604robotics.robot2020.constants.Calibration;
+import com._604robotics.robot2020.constants.Ports;
+import com._604robotics.robotnik.Action;
+import com._604robotics.robotnik.Module;
+import com._604robotics.robotnik.Output;
+import com._604robotics.robotnik.prefabs.devices.NEOEncoder;
+import com._604robotics.robotnik.prefabs.motorcontrol.Motor;
+import com._604robotics.robotnik.prefabs.motorcontrol.QuixSparkMAX;
+import com._604robotics.robotnik.prefabs.motorcontrol.controllers.MotorControllerPIDConfig;
+import com._604robotics.robotnik.prefabs.motorcontrol.controllers.SparkPID;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+
+public class Climber extends Module {
+  private QuixSparkMAX climbMotor = new QuixSparkMAX(Ports.CLIMBER_MOTOR, "Climber Motor", Motor.kNEO, this);
+
+  private NEOEncoder climbEncoder = new NEOEncoder(climbMotor);
+  private SparkPID climbPID = new SparkPID(climbMotor, climbEncoder, new MotorControllerPIDConfig(0.03, 0, 0));
+
+  public final Output<Double> climbPosition = addOutput("Climb Position", () -> climbEncoder.getPosition());
+
+  public Climber() {
+    super(Climber.class);
+    climbMotor.controller.getEncoder().setPosition(0.0);
+
+    climbMotor.controller.setIdleMode(IdleMode.kBrake);
+    climbMotor.controller.setSoftLimit(SoftLimitDirection.kForward, 365);
+
+    climbMotor.controller.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 100);
+    climbMotor.controller.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 100);
+
+    climbMotor.setCurrentLimit(60);
+
+    setDefaultAction(idle);
+  }
+
+  public class Idle extends Action {
+
+    public Idle() {
+      super(Climber.this, Idle.class);
+    }
+
+    @Override
+    public void run() {
+      climbMotor.set(0.0);
+    }
+  }
+
+  public class Extend extends Action {
+
+    public Extend() {
+      super(Climber.this, Extend.class);
+    }
+
+    @Override
+    public void begin() {
+        climbPID.setSetpointPosition(climbEncoder.getPosition() + 160);
+    }
+  }
+
+  public class Retract extends Action {
+
+    public Retract() {
+      super(Climber.this, Retract.class);
+    }
+
+    @Override
+    public void run() {
+      climbMotor.set(-0.25);
+    }
+  }
+
+  public class Climb extends Action {
+
+    public Climb() {
+      super(Climber.this, Climb.class);
+    }
+
+    @Override
+    public void run() {
+
+      climbMotor.set(Calibration.Climber.CLIMB_SPEED);
+    }
+  }
+
+  public Action idle = new Idle();
+  public Action extend = new Extend();
+  public Action retract = new Retract();
+  public Action climb = new Climb();
+}

src/com/_604robotics/robot2020/modules/Swerve.java

@@ -156,25 +156,25 @@ public class Swerve extends Module {
 
   public final Output<Double> gyroAngle = addOutput("gyroAngle", imu::getAngle);
 
-  // public final Output<Double> frontLeftAngle = addOutput("Front Left Angle", frontLeft::getAngle);
-  // public final Output<Double> frontRightAngle = addOutput("Front Right Angle", frontRight::getAngle);
-  // public final Output<Double> rearLeftAngle = addOutput("Rear Left Angle", rearLeft::getAngle);
-  // public final Output<Double> rearRightAngle = addOutput("Rear Right Angle", rearRight::getAngle);
+  public final Output<Double> frontLeftAngle = addOutput("Front Left Angle", frontLeft::getAngle);
+  public final Output<Double> frontRightAngle = addOutput("Front Right Angle", frontRight::getAngle);
+  public final Output<Double> rearLeftAngle = addOutput("Rear Left Angle", rearLeft::getAngle);
+  public final Output<Double> rearRightAngle = addOutput("Rear Right Angle", rearRight::getAngle);
 
-  // public final Output<Double> absfrontLeftAngle = addOutput("Abs Front Left Angle", frontLeft::getAbsEncoderAngle);
-  // public final Output<Double> absfrontRightAngle = addOutput("Abs Front Right Angle", frontRight::getAbsEncoderAngle);
-  // public final Output<Double> absrearLeftAngle = addOutput("Abs Rear Left Angle", rearLeft::getAbsEncoderAngle);
-  // public final Output<Double> absrearRightAngle = addOutput("Abs Rear Right Angle", rearRight::getAbsEncoderAngle);
+  public final Output<Double> absfrontLeftAngle = addOutput("Abs Front Left Angle", frontLeft::getAbsEncoderAngle);
+  public final Output<Double> absfrontRightAngle = addOutput("Abs Front Right Angle", frontRight::getAbsEncoderAngle);
+  public final Output<Double> absrearLeftAngle = addOutput("Abs Rear Left Angle", rearLeft::getAbsEncoderAngle);
+  public final Output<Double> absrearRightAngle = addOutput("Abs Rear Right Angle", rearRight::getAbsEncoderAngle);
 
   public final Output<Double> robotHeading = addOutput("Robot Heading", this::getHeadingDegrees);
   public final Output<Double> robotX = addOutput("Robot X Position", this::getX);
   public final Output<Double> robotY = addOutput("Robot Y Position", this::getY);
 
-
+  private double gyroOffset = 0.0;
 
   /* Auton Methods */
   public Rotation2d getHeading() {
-    return (Calibration.Drive.GYRO_REVERSED) ? Rotation2d.fromDegrees(360 - imu.getAngle()) : Rotation2d.fromDegrees(imu.getAngle());
+    return (Calibration.Drive.GYRO_REVERSED) ? Rotation2d.fromDegrees(360 - (imu.getAngle() - gyroOffset)) : Rotation2d.fromDegrees((imu.getAngle() - gyroOffset));
     // var angle = -imu.getAngle() * (Calibration.Drive.GYRO_REVERSED ? -1.0 : 1.0);
     // return Rotation2d.fromDegrees(Math.IEEEremainder(angle, 360));
   }
@@ -233,6 +233,10 @@ public void zeroGyro() {
     imu.reset();
   }
 
+  public void zeroGyroOffset() {
+    gyroOffset = imu.getAngle();
+  }
+
   public void setModuleStates(boolean openLoop, QuixSwerveModuleState... desiredStates) {
     QuixSwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, Calibration.Drive.MAX_DRIVE_VELOCITY);
 

src/com/_604robotics/robotnik/prefabs/devices/QuixCANCoder.java

@@ -3,6 +3,7 @@
 
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderStatusFrame;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
 
 public class QuixCANCoder implements BetterAbsoluteEncoder {
@@ -18,6 +19,9 @@ public QuixCANCoder(int port, String name) {
         this.encoder.configFactoryDefault();
         this.encoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
         this.encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
+
+        this.encoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 50);
+        this.encoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 100);
     }
 
     @Override

src/com/_604robotics/robotnik/prefabs/swerve/QuixSwerveModule.java

@@ -81,9 +81,9 @@ public QuixSwerveModule(
         this.driveEncoder.setdistancePerRotation(this.driveRatio.calculate(Math.PI * wheelDiameter));
         this.steeringEncoder.setdistancePerRotation(this.steeringRatio.calculate(360.0));
 
-        lastAngle = getState().angle.getDegrees();
-
         zeroToAbsPosition();
+
+        lastAngle = getState().angle.getDegrees();
     }
 
     // public void setDesiredStateClosedLoop(QuixSwerveModuleState desiredState) {