Merge branch 'mini'

src/main/java/com/team2502/robot2023/Constants.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -136,13 +137,16 @@ public static final class HardwareMap {
     public static final class Subsystems {
         public static final class Leds {
             public static final int PORT = 7; // pwm:
-            public static final int LED_COUNT = 50; // logical, not physical count (same on 2815)
-            public static final int FRAME_RATE = 15;
+            public static final int LED_COUNT = 84; // logical, not physical count (same on 2815)
+            public static final int FRAME_RATE = 1;
             public static final int FRAME_TIME = 1/FRAME_RATE; // seconds per frame
 
             public static final int LED_AHEAD = 0; // Led id corresponding to center of front
             public static final int LED_LEFT = LED_COUNT/4; // Led id corresponding to center of left side
 
+            public static final Color8Bit RED = new Color8Bit(255,0,0);
+            public static final Color8Bit BLU = new Color8Bit(0,255,0);
+            public static final Color8Bit WHI = new Color8Bit(188,188,188);
         }
 
         public static final class Arm {

src/main/java/com/team2502/robot2023/RobotContainer.java

@@ -52,12 +52,13 @@ public class RobotContainer {
     protected final ArmSubsystem ELEVATOR = new ArmSubsystem();
     protected final IntakeSubsystem INTAKE = new IntakeSubsystem();
 
-    protected final LightstripSubsystem LIGHTSTRIP = new LightstripSubsystem();
+    protected final LightstripSubsystem LIGHTSTRIP = new LightstripSubsystem(ELEVATOR);
 
-    //protected final PhotonVisionSubsystem VISION = new PhotonVisionSubsystem(DRIVETRAIN);
+    // protected final PhotonVisionSubsystem VISION = new PhotonVisionSubsystem(DRIVETRAIN);
 
     public RobotContainer() {
         DRIVETRAIN.setDefaultCommand(new DriveCommand(DRIVETRAIN, JOYSTICK_DRIVE_LEFT, JOYSTICK_DRIVE_RIGHT));
+        //LIGHTSTRIP.setDefaultCommand(new RunAnimationCommand(LIGHTSTRIP, LightstripSubsystem.Animations.orbit_demo_simple, 1));
 
         AutoChooser.putToSmartDashboard();
 
@@ -68,6 +69,8 @@ private void configureButtonBindings() {
         SmartDashboard.putData("RP pose b", new InstantCommand(() -> DRIVETRAIN.setPose(new Pose2d(1.9, 4.45, Rotation2d.fromDegrees(0)))));
         SmartDashboard.putData("B reset pose mid", new InstantCommand(() -> DRIVETRAIN.setPose(new Pose2d(2, 2.75, Rotation2d.fromDegrees(0))), DRIVETRAIN));
 
+        SmartDashboard.putData("test balance", new BalanceCommand(DRIVETRAIN, false));
+
         JoystickButton ResetHeading = new JoystickButton(JOYSTICK_DRIVE_RIGHT, Constants.OI.RESET_HEADING);
         ResetHeading.whenPressed(new InstantCommand(DRIVETRAIN::resetHeading, DRIVETRAIN));
 

src/main/java/com/team2502/robot2023/autonomous/Autos.java

@@ -92,6 +92,19 @@ public enum Autos { // first auto is default
                 new InstantCommand(() -> i.setSpeed(0))
         )),
 
+        ONE_CUBE_MID_ENGAGE_UNIHETERO_ETERNAL("PB center, no odo, never stop", (d,i,a) -> Commands.sequence(
+            new InstantCommand(d::resetPitch),
+            new InstantCommand(d::resetHeading),
+            Commands.deadline(Commands.waitSeconds(2.25), new SetArmSimpleCommand(a, ElevatorPosition.CUBE_TOP, IntakePosition.CUBE_TOP)),
+            Commands.deadline(Commands.waitSeconds(0.125), new InstantCommand(() -> i.setSpeed(-0.25))),
+            Commands.deadline(new InstantCommand(() -> i.setSpeed(0))),
+            Commands.deadline(Commands.waitSeconds(0.6), new SetArmSimpleCommand(a, ElevatorPosition.BOTTOM, IntakePosition.CUBE_GROUND)),
+            Commands.deadline(Commands.waitSeconds(1.5), new SetArmSimpleCommand(a, ElevatorPosition.BOTTOM, IntakePosition.INIT)),
+            Commands.deadline(Commands.waitSeconds(0.37), new YawLockedTranspose(d, new ChassisSpeeds(-.8,0,0), Mode.NAVX_ZERO)),
+            Commands.deadline(Commands.waitSeconds(0.5), new YawLockedTranspose(d, new ChassisSpeeds(-1,0,0), Mode.NAVX_ZERO)),
+            new BalanceCommand(d, false)
+        )),
+
         ONE_CUBE_MID_ENGAGE_UNIHETERO("PB center, no odo", (d,i,a) -> Commands.sequence(
             new InstantCommand(d::resetPitch),
             new InstantCommand(d::resetHeading),

src/main/java/com/team2502/robot2023/subsystems/ArmSubsystem.java

@@ -1,6 +1,5 @@
 package com.team2502.robot2023.subsystems;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel;
 import com.revrobotics.CANSparkMax.IdleMode;
@@ -130,6 +129,7 @@ public void setPitch(ElevatorPitch pitch) {
     }
 
     public void set(ElevatorPosition pos) {
+        SmartDashboard.putNumber("Elev Target", pos.position);
         rightElevator.getPIDController().setReference(pos.position, CANSparkMax.ControlType.kPosition);
     }
 
@@ -160,10 +160,12 @@ public boolean safePitch() {
     }
 
     public void setArmWrist(IntakePosition position) {
+        SmartDashboard.putNumber("Wrist Target", position.pitchWrist);
         pitchIntake.getPIDController().setReference(position.pitchWrist, CANSparkMax.ControlType.kPosition);
     }
 
     public void setArmPitch(IntakePosition position) {
+        SmartDashboard.putNumber("Elbow Target", position.pitchElbow);
         leftPitchElevator.getPIDController().setReference(position.pitchElbow, CANSparkMax.ControlType.kPosition);
     }
 
@@ -207,6 +209,26 @@ public void zeroElevator() {
         rightElevator.getEncoder().setPosition(0);
     }
 
+    public boolean nearSetPoint(Double range) {
+        /*return currentIntakePosition.pitchElbow - range <= leftPitchElevator.getEncoder().getPosition() && leftPitchElevator.getEncoder().getPosition() >= currentIntakePosition.pitchElbow + range &&
+                currentIntakePosition.pitchWrist - range <= pitchIntake.getEncoder().getPosition() && pitchIntake.getEncoder().getPosition() >= currentIntakePosition.pitchWrist + range &&
+                currentElevatorPosition.position - range <= leftElevator.getEncoder().getPosition() && leftElevator.getEncoder().getPosition() >= currentElevatorPosition.position + range;*
+         */
+
+        double elevTarget = SmartDashboard.getNumber("Elev Target", 0);
+        double actualElev = leftElevator.getEncoder().getPosition();
+
+        double pitchTarget = SmartDashboard.getNumber("Pitch Target", 0);
+        double actualPitch = leftPitchElevator.getEncoder().getPosition();
+
+        double wristTarget = SmartDashboard.getNumber("Wrist Target", 0);
+        double actualWrist = pitchIntake.getEncoder().getPosition();
+
+        return (elevTarget - range) <= actualElev && actualElev <= (elevTarget + range) &&
+                (pitchTarget - range) <= actualPitch && actualPitch <= (pitchTarget + range) &&
+                (wristTarget - range) <= actualWrist && actualWrist <= (wristTarget + range);
+    }
+
     private void NTInit() {
         SmartDashboard.putNumber("ELEVATOR_P", Constants.Subsystems.Arm.ELEVATOR_P);
         SmartDashboard.putNumber("ELEVATOR_I", Constants.Subsystems.Arm.ELEVATOR_I);

src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java

@@ -346,6 +346,8 @@ public void setSpeeds(ChassisSpeeds speed, Translation2d centerOfRotation) {
         SmartDashboard.putNumber("BL Angle", BLRotation.getDegrees());
         SmartDashboard.putNumber("BR Angle", BRRotation.getDegrees());
 
+        SmartDashboard.putNumber("Robot Pitch", getRoll());
+
         SmartDashboard.putNumber("FL mps", (drivetrainPowerFrontLeft.getSelectedSensorVelocity() / Drivetrain.FALCON_ENCODER_TICKS_PER_REV) / Drivetrain.SWERVE_DRIVE_GEAR_RATIO);
         SmartDashboard.putNumber("FL targ mps", FLState.speedMetersPerSecond);
     }
@@ -429,18 +431,18 @@ public double getHeading() {
         return navX.getAngle();
     }
 
-    public void resetPitch() {
+    public void resetRoll() {
         pitchOffset = navX.getRoll();
     }
 
-    public void resetRoll() {
+    public void resetPitch() {
         rollOffset = navX.getPitch();
     }
 
-    public double getRoll() {
+    public double getPitch() {
         return navX.getPitch()-rollOffset; // rio sideways
     }
-    public double getPitch() {
+    public double getRoll() {
         return navX.getRoll()-pitchOffset; // rio sideways
     }
 

src/main/java/com/team2502/robot2023/subsystems/LightstripSubsystem.java

@@ -3,27 +3,31 @@
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.Iterator;
-import java.util.Optional;
 import java.util.function.Function;
 
-import com.team2502.robot2023.Constants;
 import com.team2502.robot2023.Constants.Subsystems.Leds;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import static edu.wpi.first.wpilibj.DriverStation.isDisabled;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
 public class LightstripSubsystem extends SubsystemBase {
 
     /** Class for static animations not tied to a subsystem 
      * 
      * {@link Animation}s that need to accept data inputs should be defined elsewhere
      * */
+    static ArmSubsystem armSubsystem;
+
     public static final class Animations {
         public static final Animation off = ((s,f)->{
             for (int i = 0; i < Leds.LED_COUNT; i++) {
@@ -43,6 +47,37 @@ public static final class Animations {
             s.buffer.setRGB((int) (f/8)%Leds.LED_COUNT,20,148,0);
             return false;
         });
+        public static final Animation runtime = ((s,f)->{
+            boolean red = (DriverStation.getAlliance() == Alliance.Red);
+            Color8Bit alliance = red ? Leds.RED : Leds.BLU;
+
+            if (isDisabled()) {
+                for (int i = 0; i < Leds.LED_COUNT; i++) {
+                    s.buffer.setLED(i, alliance);
+                }
+            } /*else if (armSubsystem.nearSetPoint(5.)) {
+                for (int i = 0; i < Leds.LED_COUNT; i++) {
+                    // Blue is green? Perhaps this represents the friends we made along the way
+                    s.buffer.setRGB(i, 0, 0, 255);
+                }
+            }*/ else {
+                for (int i = 0; i < Leds.LED_COUNT; i++) {
+                    if (i % 2 == 0) {
+                        if ((f % 2 == 0)) {
+                            s.buffer.setLED(i, alliance);
+                        } else {
+                            s.buffer.setLED(i, Leds.WHI);
+                        }
+                    }
+                    else { if ((f % 2 == 0)) {
+                        s.buffer.setLED(i, Leds.WHI);
+                    } else {
+                        s.buffer.setLED(i, alliance);
+                    } }
+                }
+            }
+            return false;
+        });
         public static final Animation orbit_demo = ((s,f)->{
             Rotation2d angle = Rotation2d.fromDegrees(f*Leds.FRAME_TIME*90);
             Rotation2d offset = Rotation2d.fromDegrees(30);
@@ -153,36 +188,41 @@ public int compareTo(ScheduledAnimation animation) {
     ArrayList<ScheduledAnimation> animations;
     Timer frameTimer;
 
-    public LightstripSubsystem() {
+    public LightstripSubsystem(ArmSubsystem armSubsystem) {
+        this.armSubsystem = armSubsystem;
         strip = new Lightstrip(Leds.PORT,Leds.LED_COUNT);
         frameTimer = new Timer();
         frameTimer.start();
 
-        animations = new ArrayList<ScheduledAnimation>(2);
+        animations = new ArrayList<ScheduledAnimation>(1);
 
-        animations.add(new ScheduledAnimation(Animations.orbit_demo_simple, 2));
-        animations.add(new ScheduledAnimation(Animations.orbit_demo_simple, -2));
+        animations.add(new ScheduledAnimation(Animations.runtime, 1));
+        //animations.add(new ScheduledAnimation(Animations.orbit_demo_simple, -2));
     }
 
     @Override
     public void periodic() {
-        if (!frameTimer.advanceIfElapsed(1/Leds.FRAME_RATE)) {
+        double timeLeft = Timer.getMatchTime();
+        timeLeft = timeLeft < 0.5 ? 100.0 : timeLeft;
+
+        double frametime = timeLeft * (1.0/15.0);
+
+        if (!frameTimer.advanceIfElapsed(frametime)) {
             return;
         }
         Collections.sort(animations);
         Iterator<ScheduledAnimation> i = animations.iterator();
 
-        SmartDashboard.putNumber("ani count",animations.size());
         while (i.hasNext()) {
             ScheduledAnimation animation = i.next();
             animation.tick(strip);
-            strip.buffer.setRGB(5,255,148,0);
+            //strip.buffer.setRGB(5,255,148,0);
             strip.flush();
 //            if (animation.tick(strip)) {
  //               animations.remove(animation);
   //          }
         }
-        strip.buffer.setRGB(5,255,148,0);
+        //strip.buffer.setRGB(5,255,148,0);
         strip.flush();
     }