bad linkage code

src/main/java/frc/robot/RobotContainer.java

@@ -11,7 +11,7 @@
 import frc.robot.commands.PowerIntake;
 import frc.robot.commands.PowerLinkage;
 import frc.robot.commands.SetIntake;
-import frc.robot.commands.SetLinkageTalon;
+import frc.robot.commands.SetLinkage;
 import frc.robot.commands.ShootInSpeaker;
 import frc.robot.commands.PowerFlywheel;
 import frc.robot.commands.RobotOrientedDrive;
@@ -171,7 +171,7 @@ public RobotContainer() {
   private final void initializeCommands() {
     fieldOrientedDrive = new FieldOrientedDrive(drivetrain);
     robotOrientedDrive = new RobotOrientedDrive(drivetrain);
-    runExtendIntake = new RunExtendIntake(intake);
+    runExtendIntake = new RunExtendIntake(intake, linkage);
     powerIntakeReversed = new PowerIntakeReversed(intake);
     powerIntake = new PowerIntake(intake);
     powerFlywheel = new PowerFlywheel(flywheel);
@@ -204,6 +204,7 @@ private void configureDefaultCommands() {
     // flywheel.setDefaultCommand(powerFlywheel);
     // drivetrain.setDefaultCommand(fieldOrientedDrive);
     climber.setDefaultCommand(powerClimber);
+    //intake.setDefaultCommand(powerIntake);
   }
 
   /**
@@ -227,6 +228,7 @@ private void configureBindings() {
 
     operatorController.a().onTrue(levelClimbers);
     operatorController.b().onTrue(new InstantCommand(() -> climber.zeroBoth(), climber));
+    operatorController.y().toggleOnTrue(runExtendIntake);
 
     // driverController.x().whileTrue(new InstantCommand(() ->
     // drivetrain.zero(),drivetrain));
@@ -241,6 +243,7 @@ public void configureCharacterizationBindings(){
     driverController.y().whileTrue(drivetrain.sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse));
   }
   public void onDisable() {
+    climber.stop();
     flywheel.stop();
     intake.stop();
     linkage.stop();

src/main/java/frc/robot/commands/PowerIntake.java

@@ -28,13 +28,13 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // if(operatorCont.getRightTriggerAxis() > .75) {
-    //   intake.run(-.5);
-    // } else {
-    //   intake.run(-.15);
-    // }
+    if(operatorCont.getRightTriggerAxis() > .75) {
+      intake.run(.5);
+    } else {
+      intake.run(.15);
+    }
 
-    intake.run(1.0);
+    // intake.run(1.0);
     CommandLogger.logCommandRunning(this);
   }
 

src/main/java/frc/robot/commands/RunExtendIntake.java

@@ -20,24 +20,28 @@ enum IntakeCases {CHECK_ROBOT_EMPTY, EXTEND_INTAKE, WAIT_FOR_SENSOR, UP_TO_SHOOT
   private static XboxController operatorCont = new XboxController(1);
   private Timer timer = new Timer();
   private Timer sensorTimer = new Timer();
-  private double setPoint;
+  private boolean isAuto;
+  private boolean spiked = false;
 
   private IntakeCases state = IntakeCases.CHECK_ROBOT_EMPTY;
 
 
 
   /** Creates a new Java. */
-  public RunExtendIntake(Intake intake) {
+  public RunExtendIntake(Intake intake, Linkage linkage) {
     this.intake = intake;
+    this.linkage = linkage;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(intake);
+    addRequirements(intake, linkage);
   }
 
-  //   public RunExtendIntake(boolean isAuto) {
-  //   inAuto = isAuto;
-  //   // Use addRequirements() here to declare subsystem dependencies.
-  //   addRequirements(intake);
-  // }
+    public RunExtendIntake(Intake intake, Linkage linkage, boolean isAuto) {
+    this.isAuto = isAuto;
+    this.intake = intake;
+    this.linkage = linkage;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(intake, linkage);
+  }
 
   // Called when the command is initially scheduled.
   @Override
@@ -53,12 +57,14 @@ public void execute() {
     switch(state){
       case CHECK_ROBOT_EMPTY:
         if(intake.getSideSensor()) {
-          state = IntakeCases.EXTEND_INTAKE;
+          state = IntakeCases.WAIT_FOR_SENSOR;
         }
         break;
       case EXTEND_INTAKE:
-        intake.run(.65); // we should extend too but idk how we should implement this
-        //linkage.setAngle(180);
+        intake.run(.7); // we should extend too but idk how we should implement this
+        if(isAuto) {
+          linkage.setAngle(0.0); // dunno if this value is right
+        }
         if(intake.getAmps() > 20 && timer.get() > .25) {
           sensorTimer.start();
           state = IntakeCases.WAIT_FOR_SENSOR;
@@ -69,7 +75,7 @@ public void execute() {
         // if(!intake.getHighSensor()) {
         //   state = IntakeCases.UP_TO_SHOOTER_P1;
         // }
-        if(sensorTimer.get() > 1) {
+        if(sensorTimer.get() > 1.0) {
           state = IntakeCases.EXTEND_INTAKE;
           sensorTimer.reset();
         } 
@@ -154,8 +160,11 @@ public void end(boolean interrupted) {
   @Override
   public boolean isFinished() {
     if(state == IntakeCases.RETRACT_STOP) {
+      // dunno if this value is right
       System.out.print("COMMAND ENDED");
+      if(linkage.isAtSetpoint()) {
       return true;
+      }
     }
     return false;
   }

src/main/java/frc/robot/commands/SetLinkageTalon.java → src/main/java/frc/robot/commands/SetLinkage.java

@@ -8,11 +8,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Linkage;
 
-public class SetLinkageTalon extends Command {
+public class SetLinkage extends Command {
   final Linkage linkage;
   /** Creates a new SetLinkageTa\
    * lon. */
-  public SetLinkageTalon(Linkage linkage) {
+  public SetLinkage(Linkage linkage) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.linkage = linkage;
     addRequirements(linkage);

src/main/java/frc/robot/subsystems/Intake.java

@@ -64,6 +64,7 @@ public double getAmps() {
   public void periodic() {
     // This method will be called once per scheduler run
     io.updateInputs(inputs);
+    SmartDashboard.putNumber("Amps", getAmps());
     Logger.processInputs("Intake", inputs);
   }
 }

src/main/java/frc/robot/subsystems/Linkage.java

@@ -43,7 +43,7 @@ public Linkage(LinkageIO io) {
   }
 
   public boolean isAtSetpoint() {
-	  return false;
+	  return Math.abs(this.getAngle()-this.positionSetpoint) < 1.0;
   }
 
   public void run(double speed) {

src/main/java/frc/robot/utils/CommandFactory.java

@@ -50,7 +50,7 @@ public PowerLinkage powerLinkage() {
 
     // returns type runExtendIntake
     public RunExtendIntake runExtendIntake() {
-        return new RunExtendIntake(intake);
+        return new RunExtendIntake(intake, linkage);
     }
 
     // returns type runLinkage