diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json index 95f2eef4..43f54f60 100644 --- a/.DataLogTool/datalogtool.json +++ b/.DataLogTool/datalogtool.json @@ -1,6 +1,6 @@ { "download": { "localDir": "C:\\Users\\Deep Blue\\Downloads", - "serverTeam": "199" + "serverTeam": "10.1.99.2" } } diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4dd1b15c..40e68541 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -59,7 +59,7 @@ public static final class Effectorc { public static final double[] kV = { 0.122, 0/* 0.065239, 0.077913 */ }; public static final double[] kA = { 0, 0/* 0.0062809,0.05289 */ }; public static final int INTAKE_PORT = 9; // port - public static final int OUTAKE_PORT = 10; // port + public static final int OUTTAKE_PORT = 10; // port public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java index 5360739c..2a9b4722 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java @@ -10,42 +10,30 @@ public class IntakeNEO extends Command { // intake until sees game peice or 4sec has passed - private Timer timer = new Timer(); private final IntakeShooter intake; - double increaseAmount = 0.05; - int index = 0; - public int speed; public IntakeNEO(IntakeShooter intake) { addRequirements(this.intake = intake); - SmartDashboard.putNumber("Intake RPM", speed); - } @Override public void initialize() { - //TODO: Adjust speed or add in an index - timer.reset(); + // TODO: Adjust speed or add in an index; // if (intake.intakeDetectsNote()) { // return; // } - intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed)); + intake.motorSetIntake(.5); // Fast intake speed for initial intake intake.resetCurrentLimit(); - index=0; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed)); // Intake Led if((intake.intakeDetectsNote())) { - timer.start(); - } else { - timer.stop(); - timer.reset(); + intake.motorSetIntake(.1); // Slower intake speed triggered after intake ds sees note } } @@ -53,8 +41,6 @@ public void execute() { @Override public void end(boolean interrupted) { intake.stopIntake(); - timer.stop(); - index = 0; //intake.resetCurrentLimit(); } @@ -63,6 +49,6 @@ public void end(boolean interrupted) { public boolean isFinished() { // return intake.intakeDetectsNote() && timer.get()>0.1; // || //timer.hasElapsed(MAX_SECONDS_OVERLOAD); - return intake.intakeDetectsNote(); + return intake.outtakeDetectsNote(); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 6bd70b3c..f2835fb9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -27,7 +27,8 @@ public class IntakeShooter extends SubsystemBase { private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO); // private final CANSparkMax outakeMotor = // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550); - private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(10, MotorType.kBrushless); + private final CANSparkFlex outtakeMotorVortex = + new CANSparkFlex(OUTTAKE_PORT, MotorType.kBrushless); private final RelativeEncoder outtakeEncoder = outtakeMotorVortex.getEncoder(); private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); private final SparkPIDController pidControllerOutake = outtakeMotorVortex.getPIDController(); @@ -36,7 +37,6 @@ public class IntakeShooter extends SubsystemBase { private Timer intakeTOFTimer = new Timer(); private Timer outtakeTOFTimer = new Timer(); private int count = 0; - private StringLogEntry tofLogEntry; private SimpleMotorFeedforward intakeFeedforward = new SimpleMotorFeedforward(kS[INTAKE], kV[INTAKE], kA[INTAKE]); private final SimpleMotorFeedforward outtakeFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], @@ -61,8 +61,8 @@ public IntakeShooter() { pidControllerIntake.setI(kI[INTAKE]); pidControllerIntake.setD(kD[INTAKE]); SmartDashboard.putData("Intake Shooter", this); - SmartDashboard.putNumber("Intake Ks", kS[INTAKE]); - SmartDashboard.putNumber("Intake Kv", kV[INTAKE]); + // SmartDashboard.putNumber("Intake Ks", kS[INTAKE]); + // SmartDashboard.putNumber("Intake Kv", kV[INTAKE]); intakeEncoder.setAverageDepth(4); intakeEncoder.setMeasurementPeriod(8); // SmartDashboard.putNumber("intake volts", 0); @@ -71,7 +71,8 @@ public IntakeShooter() { intakeDistanceSensor.setRangingMode(RangingMode.Short, 24);// 24 ms is the minimum sample time acc to docs outtakeDistanceSensor.setRangingMode(RangingMode.Short, 24); outtakeMotorVortex.setSmartCurrentLimit(60); - SmartDashboard.putNumber("Intake target RPM", 0); + // SmartDashboard.putNumber("Intake target RPM", 0); + // SmartDashboard.putNumber("Vortex volts", 0); } public boolean intakeIsOverTemp() { @@ -141,12 +142,12 @@ public void updateValues() { @Override public void periodic() { updateValues(); - double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]); - double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]); + // double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]); + /// double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]); - if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) { - intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV); - } + // if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) { + // intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV); + // } SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote()); SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote()); SmartDashboard.putNumber("intake sample rate", intakeDistanceSensor.getSampleTime()); @@ -174,10 +175,6 @@ else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) // outakeMotor.set(SmartDashboard.getNumber("intake volts", 0)); // count++; - - // double volts = SmartDashboard.getNumber("Vortex volts", 0); - // outakeMotorVortex.set(volts); - // setMaxOutake(); SmartDashboard.putNumber("Intake amps", intakeMotor.getOutputCurrent());