Skip to content
This repository has been archived by the owner on Nov 10, 2024. It is now read-only.

Commit

Permalink
bakalım bakalım
Browse files Browse the repository at this point in the history
  • Loading branch information
MorphaxTheDeveloper committed Mar 8, 2024
1 parent df4c85c commit c04ba7f
Show file tree
Hide file tree
Showing 8 changed files with 163 additions and 47 deletions.
26 changes: 16 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@
import frc.robot.commands.Swerve.SwerveJoystickCmd;
import frc.robot.commands.common.FeedingPosition;
import frc.robot.commands.common.IntakeInputPosition;
import frc.robot.commands.common.cinarcan;
import frc.robot.commands.common.dalhacan;
import frc.robot.commands.common.fedleme;
import frc.robot.commands.feeder.FeederRunTillSwitch;
import frc.robot.subsystems.FeederSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
Expand Down Expand Up @@ -82,10 +83,10 @@ private void configureBindings() {
new JoystickButton(driverJoytick, 3).whileTrue(new ShooterSetDegree(m_shooter, 160.0));
// new JoystickButton(driverJoytick, 4).whileTrue(new cinarcan(m_intake,
// m_feeder));
new JoystickButton(driverJoytick, 4).whileTrue(new PidIntakeCommand(m_intake, 1.2));
new JoystickButton(driverJoytick, 4).whileTrue(new InstantCommand(() -> m_intake.degistir()));
new JoystickButton(driverJoytick, 1).whileFalse(new InstantCommand(() -> m_intake.StopAngleMotor()));
new JoystickButton(driverJoytick, 4).whileTrue(new RunTillSwitch(m_intake,false));
new JoystickButton(driverJoytick, 4).whileFalse(new InstantCommand(() -> m_intake.StopNoteMotor()));

new JoystickButton(driverJoytick, 1).whileFalse(new InstantCommand(() -> m_intake.StopAngleMotor()));
new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(() -> m_intake.reset()));
new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(() -> m_shooter.AngleEncoderReset()));

Expand All @@ -101,8 +102,8 @@ private void configureBindings() {
// m_feeder.backward()));
// new JoystickButton(driverJoytick, 4).whileFalse(new RunCommand(()->
// m_feeder.stop()));
new JoystickButton(driverJoytick, 9).whileTrue(new cinarcan(m_intake, m_feeder));
new JoystickButton(driverJoytick, 9).whileFalse(new cinarcan(m_intake, m_feeder));
//new JoystickButton(driverJoytick, 9).whileTrue(new cinarcan(m_intake, m_feeder));
//ew JoystickButton(driverJoytick, 9).whileFalse(new cinarcan(m_intake, m_feeder));

// new JoystickButton(driverJoytick, 1).whileTrue(new InstantCommand(()->m_shooter.AngleEncoderReset()));
//new JoystickButton(driverJoytick, 2).whileTrue(new IntakeInputPosition(m_intake));
Expand All @@ -112,17 +113,22 @@ private void configureBindings() {
// new JoystickButton(driverJoytick, 4).whileFalse(new RunCommand(()-> m_feeder.stop()));
// new JoystickButton(driverJoytick, 9).whileTrue(new cinarcan(m_intake, m_feeder));
//new JoystickButton(driverJoytick, 9).whileFalse(new cinarcan(m_intake, m_feeder));
new JoystickButton(driverJoytick, 5).whileTrue(new RunTillSwitch(m_intake));
new JoystickButton(driverJoytick, 5).whileFalse(new InstantCommand(()-> m_intake.StopNoteMotor()));
new JoystickButton(driverJoytick, 5).whileTrue(new RunTillSwitch(m_intake,false));
new JoystickButton(driverJoytick, 5).whileFalse(new InstantCommand(()->m_intake.StopNoteMotor()));


// new JoystickButton(driverJoytick, 5).onTrue(new InstantCommand(()-> m_intake.degistir()));

new JoystickButton(driverJoytick, 6).whileTrue(new InstantCommand(() -> m_feeder.runtillswitch()));
new JoystickButton(driverJoytick, 6).whileTrue(new FeederRunTillSwitch(m_feeder, false));
new JoystickButton(driverJoytick, 6).whileFalse(new InstantCommand(() -> m_feeder.stop()));

new JoystickButton(driverJoytick, 8).whileTrue(new InstantCommand(() -> m_intake.runpickupmotor(0.8)));
new JoystickButton(driverJoytick, 8).whileFalse(new InstantCommand(() -> m_intake.runpickupmotor(0)));
new JoystickButton(driverJoytick, 9).whileTrue(new InstantCommand(() -> m_feeder.backward()));
//new JoystickButton(driverJoytick, 9).whileTrue(new InstantCommand(() -> m_feeder.backward()));
new JoystickButton(driverJoytick, 9).whileFalse(new InstantCommand(() -> m_feeder.stop()));
new JoystickButton(driverJoytick, 9).whileFalse(new InstantCommand(() -> m_intake.getNote()));


}

public Command getAutonomousCommand() {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,6 @@ else if (position < -m_intake.getRawEncoderOutput()) {

@Override
public boolean isFinished() {
return false;
return getController().atSetpoint();
}
}
39 changes: 28 additions & 11 deletions src/main/java/frc/robot/commands/Intake/RunTillSwitch.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,21 +6,25 @@

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.values.intake;
import frc.robot.subsystems.IntakeSubsystem;

public class RunTillSwitch extends Command {
IntakeSubsystem m_intake;
boolean dursunmu;
boolean varmi = false;

public RunTillSwitch(IntakeSubsystem m_intake) {

public RunTillSwitch(IntakeSubsystem m_intake, boolean dursunmu) {

this.m_intake = m_intake;

}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {

}

// Called every time the scheduler runs while the command is scheduled.
@Override
Expand All @@ -33,19 +37,32 @@ public void execute() {
ilkswitch = m_intake.intakedetector1.get();
ikinciswitch = m_intake.intakedetector1.get();

if(ikinciswitch && ilkswitch){

m_intake.getNote();

}else{

m_intake.StopNoteMotor();
}
if(!dursunmu){

SmartDashboard.putBoolean("asdadssdaasd", ilkswitch);
SmartDashboard.putBoolean("asdadssdaasd2222", ikinciswitch);
if(ikinciswitch && ilkswitch){

m_intake.getNote();

}else{

m_intake.StopNoteMotor();
varmi = true;
}
}



// if(varmi){
// m_intake.getNote();
// }



SmartDashboard.putBoolean("asdadssdaasd", ilkswitch);
SmartDashboard.putBoolean("asdadssdaasd2222", ikinciswitch);
}

// Called once the command ends or is interrupted.
Expand All @@ -55,7 +72,7 @@ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return dursunmu;
}

}
21 changes: 0 additions & 21 deletions src/main/java/frc/robot/commands/common/cinarcan.java

This file was deleted.

36 changes: 36 additions & 0 deletions src/main/java/frc/robot/commands/common/fedleme.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands.common;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.Intake.PidIntakeCommand;
import frc.robot.commands.feeder.FeederRunTillSwitch;
import frc.robot.subsystems.FeederSubsystem;
import frc.robot.subsystems.IntakeSubsystem;

// 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 fedleme extends SequentialCommandGroup {
/** Creates a new fedleme. */
public fedleme(IntakeSubsystem m_intake, FeederSubsystem m_feeder) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(
new PidIntakeCommand(m_intake, 1.8)

.andThen(new RunCommand(()-> m_intake.getNote())
.andThen(new FeederRunTillSwitch(m_feeder, false)
))
.withTimeout(2)




);
}
}
65 changes: 65 additions & 0 deletions src/main/java/frc/robot/commands/feeder/FeederRunTillSwitch.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands.feeder;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.FeederSubsystem;

public class FeederRunTillSwitch extends Command {
FeederSubsystem m_feeder;
boolean dursunmu;
boolean varmi = false;


public FeederRunTillSwitch(FeederSubsystem m_feeder, boolean dursunmu) {

this.m_feeder = m_feeder;

}

// Called when the command is initially scheduled.
@Override
public void initialize() {

}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {


boolean ilkswitch;

ilkswitch = m_feeder.detector();


if(!dursunmu){

if(ilkswitch){

m_feeder.backward2();

}else{

m_feeder.stop();
varmi = true;
}
}



}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return dursunmu;
}

}
14 changes: 13 additions & 1 deletion src/main/java/frc/robot/subsystems/FeederSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@ public class FeederSubsystem extends SubsystemBase {
public boolean count;
IntakeSubsystem m_intake;
boolean tekcalisma ;
boolean dolumu = false;

//public AnalogInput intechedetector = new AnalogInput(0);


Expand All @@ -23,8 +25,18 @@ public FeederSubsystem() {

}


public boolean detector() {
return detector.get();
}


public void forward() {
feedmotor.set(0.8);
feedmotor.set(0.3);
}

public void backward2() {
feedmotor.set(-0.36);
}

public void backward() {
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkBase.IdleMode;
Expand All @@ -11,9 +12,10 @@

public class IntakeSubsystem extends SubsystemBase {
// intake nota motorları
public static CANSparkMax intakeMotor1 = new CANSparkMax(12, MotorType.kBrushless);
public static WPI_VictorSPX intakeMotor1 = new WPI_VictorSPX(12);
// intake main motor
public static CANSparkMax intakeMotor = new CANSparkMax(Constants.ports.intake_motor_angle, MotorType.kBrushless);

public RelativeEncoder IntakeEncoder;
public DigitalInput intakedetector1 = new DigitalInput(8);
public DigitalInput intakedetector2 = new DigitalInput(9);
Expand Down Expand Up @@ -50,7 +52,7 @@ public void NewIntakeMotorOutput(double value) {
}

public void getNote() {
intakeMotor1.set(0.3);
intakeMotor1.set(0.4);
}

public void degistir() {
Expand Down Expand Up @@ -101,6 +103,5 @@ public void periodic() {
SmartDashboard.putBoolean("intake switch2", intakeswitch2());

SmartDashboard.putBoolean("degistirme", tekcalisma);

}
}

0 comments on commit c04ba7f

Please sign in to comment.