Merge pull request #16 from LakeEffectRobotics/durham-changes

Durham changes > master
LakeEffectRobotics · Mar 21, 2024 · 5fab191 · 5fab191
2 parents da9aafe + af9e8e8
commit 5fab191
Show file tree

Hide file tree

Showing 18 changed files with 307 additions and 72 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -46,6 +47,7 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+
   }
 
   @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -14,10 +15,13 @@
 import frc.robot.commands.IntakeCommandGroup;
 import frc.robot.commands.TransportCommandGroup;
 import frc.robot.commands.TrapCommandGroup;
+import frc.robot.commands.autonomous.DriveForDuration;
 import frc.robot.commands.instant.ClimbPrepareCommand;
+import frc.robot.commands.instant.IntakeClawCommand;
 import frc.robot.commands.instant.RetractArmCommand;
 import frc.robot.commands.instant.ShiftDownCommand;
 import frc.robot.commands.instant.ShiftUpCommand;
+import frc.robot.commands.instant.ShootClawCommand;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.Climber;
@@ -37,8 +41,7 @@ public class RobotContainer {
   public Gyro gyro = new Gyro();
 
   public RobotContainer() {
-
-    compressor.enableAnalog(90, 100);
+    compressor.enableAnalog(90, 120);
 
     configureBindings();
 
@@ -64,8 +67,8 @@ private void configureBindings() {
     //OI.spinArmButton.whileTrue(new ArmCommand(arm, OI.spinArmSpeedSupplier.getAsDouble()));
 
     /* Operator Bindings */
-    OI.spinOutClawButton.whileTrue(new ClawCommand(claw, OI.spinOutClawSpeedSupplier.getAsDouble()));
-    OI.spinInClawButton.whileTrue(new ClawCommand(claw, -OI.spinInClawSpeedSupplier.getAsDouble()));
+    OI.spinOutClawButton.whileTrue(new ShootClawCommand(claw));
+    OI.spinInClawButton.whileTrue(new IntakeClawCommand(claw, arm, OI.spinInClawSpeedSupplier.getAsDouble()));
 
     // OI.extendArmButton.onTrue(Commands.runOnce(() -> {
     //   wrist.setTargetAngle(100);
@@ -75,14 +78,15 @@ private void configureBindings() {
     // }));
     OI.intakePositionButton.onTrue(new IntakeCommandGroup(wrist, arm));
     OI.transportPositionButton.onTrue(new TransportCommandGroup(wrist, arm));
-    OI.ampPositionButton.onTrue(new AmpCommandGroup(wrist, arm));
-    OI.trapPositionButton.onTrue(new TrapCommandGroup(wrist, arm));
+    // OI.ampPositionButton.onTrue(new AmpCommandGroup(wrist, arm));
+    // OI.trapPositionButton.onTrue(new TrapCommandGroup(wrist, arm));
     OI.prepareClimbButton.onTrue(Commands.runOnce(() -> climber.prepareClimb(),climber) );
     OI.climbButton.onTrue(Commands.runOnce(() -> climber.climb(),climber));
 
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    //return Commands.print("No autonomous command configured");
+    return new DriveForDuration(drivetrain);//.raceWith(new IntakeCommandGroup(wrist, arm).andThen(new IntakeClawCommand(claw, arm, 0.25)));
   }
 }
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
@@ -38,11 +38,11 @@ private class CAN {
     }
 
     private class PCM {
-        private static final int DRIVE_SHIFT_UP = 14;
-        private static final int DRIVE_SHIFT_DOWN = 15;
+        private static final int DRIVE_SHIFT_UP = 15;
+        private static final int DRIVE_SHIFT_DOWN = 14;
 
-        private static final int ARM_PISTON_UP = 1;
-        private static final int ARM_PISTON_DOWN = 2;
+        private static final int ARM_PISTON_UP = 13;
+        private static final int ARM_PISTON_DOWN = 12;
 
         private static final int CLIMB_PISTON_UP = 10; // Todo:  have to find out the real number
         private static final int CLIMB_PISTON_DOWN = 11; // Todo:  find the real number
@@ -82,23 +82,46 @@ private class PCM {
         rightController2.follow(rightController1);
 
         leftController1.setInverted(false);
-        rightController1.setInverted(true);   
+        rightController1.setInverted(false);   
 
         /** Intake */
 
         // Climber 
         climbController2.follow(climbController1);
         climbController3.follow(climbController1);
 
+        climbController1.setIdleMode(IdleMode.kBrake);
+        climbController2.setIdleMode(IdleMode.kBrake);
+        climbController3.setIdleMode(IdleMode.kBrake);
+
         armController2.follow(armController1);
 
+        armController1.setInverted(false);
+        armController2.setInverted(false);
+
         armController1.setIdleMode(IdleMode.kCoast);
         armController2.setIdleMode(IdleMode.kCoast);
 
 
         wristController.setIdleMode(IdleMode.kBrake);
-        wristController.setInverted(true);
-
+        wristController.setInverted(true);        
+    }
+
+    public static void burnFlash(){
+        leftController1.burnFlash();
+        leftController2.burnFlash();
+        rightController1.burnFlash();
+        rightController2.burnFlash();
+
+        climbController1.burnFlash();
+        climbController2.burnFlash();
+        climbController3.burnFlash();
+
+        armController1.burnFlash();
+        armController2.burnFlash();
+
+        clawController.burnFlash();
+        wristController.burnFlash();
     }
 
 }
diff --git a/src/main/java/frc/robot/commands/AmpCommandGroup.java b/src/main/java/frc/robot/commands/AmpCommandGroup.java
@@ -20,9 +20,10 @@ public AmpCommandGroup(Wrist wrist, Arm arm) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(new WristCommand(wrist, WristPosition.UP),
-                                new RotateArmCommand(arm,ArmPosition.AMP),
-                                new ExtendArmCommand(arm, ArmExtension.RETRACT),
-                                new WristCommand(wrist, WristPosition.DOWN) 
+      new ExtendArmCommand(arm, ArmExtension.RETRACT),
+      new RotateArmCommand(arm,ArmPosition.MIDDLE),
+      new WristCommand(wrist, WristPosition.AMP),
+      new RotateArmCommand(arm, ArmPosition.AMP)
     );
   }
 }
diff --git a/src/main/java/frc/robot/commands/ExtendArmCommand.java b/src/main/java/frc/robot/commands/ExtendArmCommand.java
@@ -17,6 +17,7 @@ public ExtendArmCommand(Arm arm,Arm.ArmExtension extend) {
 
     @Override
     public void initialize(){
+        System.out.println("extend arm command initialize");
         if(extend == ArmExtension.EXTEND){
             arm.extendArm();
         }else{

diff --git a/src/main/java/frc/robot/commands/IntakeCommandGroup.java b/src/main/java/frc/robot/commands/IntakeCommandGroup.java
@@ -29,7 +29,7 @@ public IntakeCommandGroup(Wrist wrist, Arm arm) {
     addCommands(new WristCommand(wrist, WristPosition.UP),
                                 new RotateArmCommand(arm,ArmPosition.INTAKE),
                                 new ExtendArmCommand(arm, ArmExtension.EXTEND),
-                                new WristCommand(wrist, WristPosition.DOWN)
+                                new WristCommand(wrist, WristPosition.INTAKE)
     );
   }
 }
diff --git a/src/main/java/frc/robot/commands/RotateArmCommand.java b/src/main/java/frc/robot/commands/RotateArmCommand.java
@@ -14,7 +14,9 @@ public RotateArmCommand(Arm arm,Arm.ArmPosition pos) {
         this.pos = pos;
     }
     @Override
-    public void initialize(){
+    public void initialize(){ 
+        /*
+        System.out.println("rotate arm command initialize");
 
         switch (pos) {
             case INTAKE:
@@ -24,13 +26,17 @@ public void initialize(){
                     arm.rotateToTrapPos();
                 break;
             case AMP:
+                    System.out.println("ROTATE ARM COMMAND ???????????????????????????????????????");
                     arm.rotateToAmpPos();
                 break;
+            case MIDDLE:
+                    arm.rotateToMidPos(); 
+                break;
         
             default:
                 break;
         }
-
+        */
     }
 
     @Override
@@ -40,12 +46,15 @@ public void execute() {
 
     // Called once the command ends or is interrupted.
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        System.out.println("ARM COMMAND ENDEDEDEDEDEDEDEDEDEDEDEDEDEDEDED");
+    }
 
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-        return arm.inPosition();
+        return true;
+        // return arm.inPosition();
     }
 }
 
diff --git a/src/main/java/frc/robot/commands/TransportCommandGroup.java b/src/main/java/frc/robot/commands/TransportCommandGroup.java
@@ -4,6 +4,8 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Wrist;
@@ -19,9 +21,14 @@ public class TransportCommandGroup extends SequentialCommandGroup {
   public TransportCommandGroup(Wrist wrist, Arm arm) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands(new WristCommand(wrist, WristPosition.UP),
-                                new RotateArmCommand(arm,ArmPosition.INTAKE),
-                                new ExtendArmCommand(arm, ArmExtension.RETRACT)
+    addCommands(
+      new ConditionalCommand(
+        new RotateArmCommand(arm, ArmPosition.MIDDLE),
+        Commands.none(),
+        () -> arm.getCurrentAngle() > 45),  
+      new WristCommand(wrist, WristPosition.UP),
+      new RotateArmCommand(arm,ArmPosition.INTAKE),
+      new ExtendArmCommand(arm, ArmExtension.RETRACT)
     );
   }
 }
diff --git a/src/main/java/frc/robot/commands/TrapCommandGroup.java b/src/main/java/frc/robot/commands/TrapCommandGroup.java
@@ -20,8 +20,10 @@ public TrapCommandGroup(Wrist wrist, Arm arm) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(new WristCommand(wrist, WristPosition.UP),
-                                new RotateArmCommand(arm,ArmPosition.TRAP),
-                                new ExtendArmCommand(arm, ArmExtension.EXTEND)
+      new RotateArmCommand(arm,ArmPosition.MIDDLE),
+      new WristCommand(wrist, WristPosition.TRAP),
+      new RotateArmCommand(arm,ArmPosition.AMP),
+      new ExtendArmCommand(arm, ArmExtension.EXTEND)
     );
   }
 }
diff --git a/src/main/java/frc/robot/commands/WristCommand.java b/src/main/java/frc/robot/commands/WristCommand.java
@@ -4,8 +4,10 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.SerialPort.WriteBufferMode;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Wrist;
+import frc.robot.subsystems.Wrist.WristPosition;
 
 public class WristCommand extends Command {
 
@@ -23,11 +25,16 @@ public WristCommand(Wrist wrist, Wrist.WristPosition pos) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    System.out.println("wrist command initialize");
 
-    if(pos == Wrist.WristPosition.UP){  // if we are asking to move the wrist up then do it
+    if(pos == Wrist.WristPosition.UP){ 
       wrist.moveWristUp();
-    } else {
-      wrist.moveWristDown();  // otherwise it has to be down.
+    } else if (pos == WristPosition.INTAKE) {
+      wrist.moveWristIntake();
+    } else if (pos == WristPosition.TRAP) {
+      wrist.moveWristTrap();
+    } else if (pos == WristPosition.AMP) {
+      wrist.moveWristAmp();
     }
 
   }

diff --git a/src/main/java/frc/robot/commands/autonomous/DriveForDuration.java b/src/main/java/frc/robot/commands/autonomous/DriveForDuration.java
@@ -0,0 +1,33 @@
+package frc.robot.commands.autonomous;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Drivetrain;
+
+public class DriveForDuration extends Command {
+
+    Drivetrain drivetrain;
+    private Long timeout;
+
+    public DriveForDuration(Drivetrain drivetrain) {
+        addRequirements(drivetrain);
+        this.drivetrain = drivetrain;
+    }
+    @Override
+    public void initialize() {
+        timeout = System.currentTimeMillis()+2500;
+    }
+
+    @Override
+    public void execute() {
+        drivetrain.setOutput(-0.25, -0.25);
+    }
+
+    @Override
+    public void end(boolean isInterrupted){
+        drivetrain.setOutput(0, 0);
+    }
+
+    public boolean isFinished() {
+        return System.currentTimeMillis()>=timeout;
+    }
+}
diff --git a/src/main/java/frc/robot/commands/instant/IntakeClawCommand.java b/src/main/java/frc/robot/commands/instant/IntakeClawCommand.java
@@ -4,25 +4,64 @@
 
 package frc.robot.commands.instant;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.OI;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Claw;
-import frc.robot.subsystems.Claw.ClawSpeeds;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class IntakeClawCommand extends InstantCommand {
+
+
+public class IntakeClawCommand extends Command{
 
   Claw claw;
+  Arm arm;
+  double speed;
+
+  double benchSpeed = 0;
 
-  public IntakeClawCommand(Claw claw) {
+  public IntakeClawCommand(Claw claw, Arm arm, double triggerpos) {
     addRequirements(claw); 
     this.claw = claw;
+    this.arm = arm;
+  }
+
+  @Override
+  public void initialize(){
+    benchSpeed = -1;
   }
 
-  // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-    claw.setMode(ClawSpeeds.INTAKE);
+  public void execute() {
+    //double triggerPosition = OI.spinInClawSpeedSupplier.getAsDouble();
+    //System.out.println(triggerPosition);
+      speed = Claw.CLAWINTAKESPEED;
+
+      benchSpeed = Math.max(benchSpeed, claw.getSpeed());
+      SmartDashboard.putNumber("Bench", benchSpeed);
+
+      if(arm.getCurrentAngle() > 45){
+        // Outtaking
+        claw.setOutput(speed);
+      }
+      else {
+        // Intaking
+        // SmartDashboard.putNumber(claw., speed)
+        if(claw.hasNote()){
+          claw.setOutput(0);
+        } else {
+          claw.setOutput(speed);
+        }
+      }
+  }
+
+  @Override
+  public void end(boolean isInterrupted){
+    claw.setOutput(0);
   }
+
 }