From 639a4c301c9257ef245096a658e2a968971eab3f Mon Sep 17 00:00:00 2001 From: AvidhBavkar Date: Sun, 11 Mar 2018 21:44:32 -0700 Subject: [PATCH] Arizona North Day 3: Teleop and switch autonomous are fully functional Following our elimination from competitive play at the regional, software was allowed some much needed one on one time with the competition bot on a practice field. All of the Telop bugs have been ironed out per our testing, and our switch autonomous is fully functional. GnarlyController and MotionProfileCommand have been debugged and tuned to very precisely follow a trajectory and scale autonomous is almost ready for testing. --- ...ablePIDTuner.java => SendablePIDTuner.java | 106 ++++---- src/com/team3925/frc2018/Logger.java | 41 ++- src/com/team3925/frc2018/OI.java | 10 +- src/com/team3925/frc2018/Robot.java | 16 +- .../frc2018/commands/DriveManual.java | 150 +++++------ .../frc2018/commands/RunElevator.java | 67 ++--- .../commands/autos/CenterSwitchAuto.java | 25 +- .../frc2018/commands/autos/LeftScaleAuto.java | 38 +++ .../team3925/frc2018/subsystems/Elevator.java | 81 ++++-- .../team3925/frc2018/subsystems/Intake.java | 7 +- .../team3925/utils/MotionProfileCommand.java | 252 +++++++++--------- 11 files changed, 440 insertions(+), 353 deletions(-) rename src/com/team3925/utils/SendablePIDTuner.java => SendablePIDTuner.java (96%) create mode 100644 src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java diff --git a/src/com/team3925/utils/SendablePIDTuner.java b/SendablePIDTuner.java similarity index 96% rename from src/com/team3925/utils/SendablePIDTuner.java rename to SendablePIDTuner.java index 2ba4406..ab23e7d 100644 --- a/src/com/team3925/utils/SendablePIDTuner.java +++ b/SendablePIDTuner.java @@ -1,54 +1,54 @@ -package com.team3925.utils; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class SendablePIDTuner implements Sendable { - - String name = "PIDTuner"; - String subsystem; - PIDTunable pid_values; - - public SendablePIDTuner(Subsystem subsystem, PIDTunable pid_values) { - this.subsystem = subsystem.getName() + "Tuner"; - this.pid_values = pid_values; - } - - @Override - public String getName() { - return name; - } - - @Override - public void setName(String name) { - this.name = name; - - } - - @Override - public String getSubsystem() { - return subsystem; - } - - @Override - public void setSubsystem(String subsystem) { - this.subsystem = subsystem; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("PIDController"); - builder.addDoubleProperty("p", pid_values::getkP, pid_values::setkP); - builder.addDoubleProperty("i", pid_values::getkI, pid_values::setkI); - builder.addDoubleProperty("d", pid_values::getkD, pid_values::setkD); - builder.addDoubleProperty("f", pid_values::getkF, pid_values::setkF); - } - - public void updateDashboard() { - // TODO: should this be name or subsystem? The subsystem will allow us to - // differentiate it. - SmartDashboard.putData(subsystem, this); - } +package com.team3925.utils; + +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SendablePIDTuner implements Sendable { + + String name = "PIDTuner"; + String subsystem; + PIDTunable pid_values; + + public SendablePIDTuner(Subsystem subsystem, PIDTunable pid_values) { + this.subsystem = subsystem.getName() + "Tuner"; + this.pid_values = pid_values; + } + + @Override + public String getName() { + return name; + } + + @Override + public void setName(String name) { + this.name = name; + + } + + @Override + public String getSubsystem() { + return subsystem; + } + + @Override + public void setSubsystem(String subsystem) { + this.subsystem = subsystem; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.addDoubleProperty("p", pid_values::getkP, pid_values::setkP); + builder.addDoubleProperty("i", pid_values::getkI, pid_values::setkI); + builder.addDoubleProperty("d", pid_values::getkD, pid_values::setkD); + builder.addDoubleProperty("f", pid_values::getkF, pid_values::setkF); + } + + public void updateDashboard() { + // TODO: should this be name or subsystem? The subsystem will allow us to + // differentiate it. + SmartDashboard.putData(subsystem, this); + } } \ No newline at end of file diff --git a/src/com/team3925/frc2018/Logger.java b/src/com/team3925/frc2018/Logger.java index fbefc82..4ab1008 100644 --- a/src/com/team3925/frc2018/Logger.java +++ b/src/com/team3925/frc2018/Logger.java @@ -1,22 +1,19 @@ -package com.team3925.frc2018; - -import com.team3925.frc2018.subsystems.Drivetrain; -import com.team3925.utils.SendablePIDTuner; - -public class Logger { - SendablePIDTuner drivetrainTuner; - - private static Logger instance; - public static Logger getInstance() { - if (instance == null) - instance = new Logger(); - return instance; - } - - private Logger() { - drivetrainTuner = new SendablePIDTuner(Drivetrain.getInstance(), Drivetrain.getInstance()); - } - public void update() { - drivetrainTuner.updateDashboard(); - } -} +package com.team3925.frc2018; + +public class Logger { +// SendablePIDTuner drivetrainTuner; + + private static Logger instance; + public static Logger getInstance() { + if (instance == null) + instance = new Logger(); + return instance; + } + + private Logger() { +// drivetrainTuner = new SendablePIDTuner(Drivetrain.getInstance(), Drivetrain.getInstance()); + } + public void update() { +// drivetrainTuner.updateDashboard(); + } +} diff --git a/src/com/team3925/frc2018/OI.java b/src/com/team3925/frc2018/OI.java index 6437d94..fb89b24 100644 --- a/src/com/team3925/frc2018/OI.java +++ b/src/com/team3925/frc2018/OI.java @@ -142,7 +142,11 @@ protected void end() { @Override protected boolean isFinished() { - return Intake.getInstance().isAtSetpoint(); + if(Elevator.state == ElevatorState.BOTTOM) { + return Intake.getInstance().isAtSetpoint(); + }else { + return true; + } } }); shootCube.whenInactive(new Command() { @@ -150,7 +154,9 @@ protected boolean isFinished() { @Override protected void initialize() { Intake.getInstance().setIntakeRollers(0); - Intake.getInstance().setAngle(85); + if (Elevator.state == ElevatorState.BOTTOM) { + Intake.getInstance().setAngle(85); + } } @Override protected boolean isFinished() { diff --git a/src/com/team3925/frc2018/Robot.java b/src/com/team3925/frc2018/Robot.java index cb005e3..e22efee 100644 --- a/src/com/team3925/frc2018/Robot.java +++ b/src/com/team3925/frc2018/Robot.java @@ -8,6 +8,7 @@ import com.team3925.frc2018.commands.autos.CenterSwitchAuto; import com.team3925.frc2018.commands.autos.CenterSwitchAuto.AutoSide; import com.team3925.frc2018.commands.autos.DriveForwardAuto; +import com.team3925.frc2018.commands.autos.LeftScaleAuto; import com.team3925.frc2018.subsystems.Drivetrain; import com.team3925.frc2018.subsystems.Elevator; import com.team3925.frc2018.subsystems.Intake; @@ -31,7 +32,7 @@ public class Robot extends IterativeRobot { Command zeroIntake; UsbCamera camera; SendableChooser autoSelector; - boolean isElevatorZeroed = false; + private static boolean isElevatorZeroed; @Override public void robotInit() { @@ -45,27 +46,30 @@ public void robotInit() { camera.setFPS(12); autoSelector.addDefault("Drive Forward", "DriveForward"); autoSelector.addObject("CenterSwitch", "Center"); + autoSelector.addObject("Left Scale", "LeftScale"); SmartDashboard.putData("Auto", autoSelector); isElevatorZeroed = false; } @Override public void autonomousInit() { - isElevatorZeroed = true; + Intake.getInstance().setIntakeRollers(0.25); Elevator.getInstance().zero(); Intake.getInstance().zeroLift(); testAuto = new DriveForwardAuto(); + System.out.println(DriverStation.getInstance().getGameSpecificMessage().charAt(0)); if (autoSelector.getSelected().equals("DriveForward")) { testAuto = new DriveForwardAuto(); }else if(autoSelector.getSelected().equals("Center")){ if (DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'L') { testAuto = new CenterSwitchAuto(AutoSide.LEFT); - System.out.println("RUNnningLEFt"); }else if(DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'R') { testAuto = new CenterSwitchAuto(AutoSide.RIGHT); } } + testAuto = new LeftScaleAuto(); testAuto.start(); + isElevatorZeroed = true; } @Override @@ -73,12 +77,13 @@ public void autonomousPeriodic() { } public void teleopInit() { - if (!isElevatorZeroed) { + if (isElevatorZeroed == false) { Elevator.getInstance().zero(); Intake.getInstance().zeroLift(); + System.out.println("ZeroCalled"); } drive.start(); - Intake.getInstance().setIntakeRollers(0.3); + Intake.getInstance().setIntakeRollers(0.25); // Elevator.getInstance().setPosition(ElevatorState.BOTTOM); // Intake.getInstance().zeroLift(); } @@ -98,6 +103,5 @@ public void robotPeriodic() { @Override public void disabledInit() { - isElevatorZeroed = false; } } diff --git a/src/com/team3925/frc2018/commands/DriveManual.java b/src/com/team3925/frc2018/commands/DriveManual.java index 1efda70..a487d7e 100644 --- a/src/com/team3925/frc2018/commands/DriveManual.java +++ b/src/com/team3925/frc2018/commands/DriveManual.java @@ -1,75 +1,75 @@ -package com.team3925.frc2018.commands; - -import javax.print.attribute.standard.PrinterResolution; - -import com.team3925.frc2018.subsystems.Drivetrain; -import com.team3925.frc2018.subsystems.Elevator; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveManual extends Command { - - public interface DriveManualInput { - public abstract double getFwd(); - - public abstract double getLeft(); - } - - private final boolean doReverseWhenReversing; - - private DriveManualInput input; - private double prelimLeft, prelimRight; - private double fwd, turn; - private double scale; - private static final double K_HEIGHT_SUBTRACTION = 0.9; - - public DriveManual(DriveManualInput input) { - this.input = input; - doReverseWhenReversing = false; - requires(Drivetrain.getInstance()); - } - - @Override - protected void initialize() { - Drivetrain.getInstance().setRaw(0, 0); - } - - @Override - protected void execute() { - fwd = input.getFwd(); - turn = input.getLeft(); - if (Math.abs(fwd) < 0.1) - fwd = 0; - if (doReverseWhenReversing && fwd != 0) - turn *= Math.signum(fwd); - prelimLeft = fwd + turn; - prelimRight = fwd - turn; - if (prelimLeft != 0 || prelimRight != 0) { - scale = Math.max(Math.abs(fwd), Math.abs(turn)) / Math.max(Math.abs(prelimLeft), Math.abs(prelimRight)); - prelimLeft *= scale; - prelimRight *= scale; - } - - Drivetrain.getInstance().setRaw( - prelimLeft * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)), - prelimRight * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)) - ); - } - - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - Drivetrain.getInstance().setRaw(0, 0); - } - - @Override - protected void interrupted() { - Drivetrain.getInstance().setRaw(0, 0); - } - -} +package com.team3925.frc2018.commands; + +import javax.print.attribute.standard.PrinterResolution; + +import com.team3925.frc2018.subsystems.Drivetrain; +import com.team3925.frc2018.subsystems.Elevator; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveManual extends Command { + + public interface DriveManualInput { + public abstract double getFwd(); + + public abstract double getLeft(); + } + + private final boolean doReverseWhenReversing; + + private DriveManualInput input; + private double prelimLeft, prelimRight; + private double fwd, turn; + private double scale; + private static final double K_HEIGHT_SUBTRACTION = 0.8; + + public DriveManual(DriveManualInput input) { + this.input = input; + doReverseWhenReversing = false; + requires(Drivetrain.getInstance()); + } + + @Override + protected void initialize() { + Drivetrain.getInstance().setRaw(0, 0); + } + + @Override + protected void execute() { + fwd = input.getFwd(); + turn = input.getLeft(); + if (Math.abs(fwd) < 0.1) + fwd = 0; + if (doReverseWhenReversing && fwd != 0) + turn *= Math.signum(fwd); + prelimLeft = fwd + turn; + prelimRight = fwd - turn; + if (prelimLeft != 0 || prelimRight != 0) { + scale = Math.max(Math.abs(fwd), Math.abs(turn)) / Math.max(Math.abs(prelimLeft), Math.abs(prelimRight)); + prelimLeft *= scale; + prelimRight *= scale; + } + + Drivetrain.getInstance().setRaw( + prelimLeft * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)), + prelimRight * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)) + ); + } + + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + Drivetrain.getInstance().setRaw(0, 0); + } + + @Override + protected void interrupted() { + Drivetrain.getInstance().setRaw(0, 0); + } + +} diff --git a/src/com/team3925/frc2018/commands/RunElevator.java b/src/com/team3925/frc2018/commands/RunElevator.java index 1243c01..a9c502c 100644 --- a/src/com/team3925/frc2018/commands/RunElevator.java +++ b/src/com/team3925/frc2018/commands/RunElevator.java @@ -1,33 +1,36 @@ -package com.team3925.frc2018.commands; - -import com.team3925.frc2018.OI; -import com.team3925.frc2018.subsystems.Elevator; -import com.team3925.frc2018.subsystems.Intake; -import com.team3925.frc2018.subsystems.Elevator.ElevatorState; - -import edu.wpi.first.wpilibj.command.Command; - -public class RunElevator extends Command { - - private ElevatorState state; - - public RunElevator(ElevatorState state) { - this.state = state; - requires(Elevator.getInstance()); - } - - @Override - protected void initialize() { - Elevator.getInstance().setPosition(state); - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - Intake.getInstance().setAngle(0); - } +package com.team3925.frc2018.commands; + +import com.team3925.frc2018.subsystems.Elevator; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.frc2018.subsystems.Intake; + +import edu.wpi.first.wpilibj.command.Command; + +public class RunElevator extends Command { + + private ElevatorState state; + + public RunElevator(ElevatorState state) { + this.state = state; + requires(Elevator.getInstance()); + } + + @Override + protected void initialize() { + Elevator.getInstance().setPosition(state); + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + if (Elevator.state == (ElevatorState.TOP)) { + Intake.getInstance().setAngle(45); + }else { + Intake.getInstance().setAngle(0); + } + } } \ No newline at end of file diff --git a/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java b/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java index 7146492..6a6fbd0 100644 --- a/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java +++ b/src/com/team3925/frc2018/commands/autos/CenterSwitchAuto.java @@ -5,9 +5,11 @@ import com.team3925.frc2018.commands.RunElevator; import com.team3925.frc2018.commands.RunIntakeWheels; import com.team3925.frc2018.commands.ZeroIntake; +import com.team3925.frc2018.subsystems.Intake; import com.team3925.frc2018.subsystems.Elevator.ElevatorState; import com.team3925.utils.MotionProfileCommand; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.WaitCommand; @@ -18,16 +20,25 @@ public enum AutoSide{ } public CenterSwitchAuto(AutoSide side) { + addParallel(new Command() { + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + Intake.getInstance().setIntakeRollers(0.25); + return true; + } + }); addParallel(new CloseGrabbers()); - addParallel(new RunElevator(ElevatorState.SWITCH)); - if (side == AutoSide.LEFT) { - addParallel(new MotionProfileCommand("CENTER_LEFTSWITCH")); - }else { - addParallel(new MotionProfileCommand("CENTER_RIGHTSWITCH")); + if (side.equals(AutoSide.LEFT)) { + addSequential(new MotionProfileCommand("CENTER_LEFTSWITCH")); + }else if (side.equals(AutoSide.RIGHT)){ + addSequential(new MotionProfileCommand("CENTER_RIGHTSWITCH")); } - addSequential(new WaitCommand(2)); + addSequential(new WaitCommand(4)); + addParallel(new RunElevator(ElevatorState.SWITCH)); addSequential(new OpenGrabbers()); - addSequential(new RunIntakeWheels(-0.5)); + addSequential(new RunIntakeWheels(-0.25)); } } diff --git a/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java b/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java new file mode 100644 index 0000000..4d38a0c --- /dev/null +++ b/src/com/team3925/frc2018/commands/autos/LeftScaleAuto.java @@ -0,0 +1,38 @@ +package com.team3925.frc2018.commands.autos; + +import com.team3925.frc2018.commands.CloseGrabbers; +import com.team3925.frc2018.commands.OpenGrabbers; +import com.team3925.frc2018.commands.RunElevator; +import com.team3925.frc2018.commands.RunIntakeWheels; +import com.team3925.frc2018.commands.ZeroIntake; +import com.team3925.frc2018.subsystems.Intake; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.utils.MotionProfileCommand; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class LeftScaleAuto extends CommandGroup { + + public enum AutoSide { + LEFT, RIGHT + } + + public LeftScaleAuto() { + addParallel(new Command() { + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + Intake.getInstance().setIntakeRollers(0.25); + return true; + } + }); + addParallel(new CloseGrabbers()); + addParallel(new RunElevator(ElevatorState.SCALE_MAX)); + addSequential(new MotionProfileCommand("LEFT_LEFTSCALE")); + addSequential(new RunIntakeWheels(-1)); + } + +} diff --git a/src/com/team3925/frc2018/subsystems/Elevator.java b/src/com/team3925/frc2018/subsystems/Elevator.java index 11e6b30..12c0fd2 100644 --- a/src/com/team3925/frc2018/subsystems/Elevator.java +++ b/src/com/team3925/frc2018/subsystems/Elevator.java @@ -12,29 +12,31 @@ public class Elevator extends Subsystem { - public enum ElevatorState{ + public enum ElevatorState { TOP, SCALE_MAX, SCALE_MED, SCALE_LOW, SWITCH, BOTTOM, UNKNOWN, OTHER; } - + private final TalonSRX elevatorMaster = RobotMap.ElevatorMap.MASTER; private static final double K_ELEVATOR = 1176.48; - + public static final double TELEOP_ELEVATOR_INCREMENT = 8; + + public static ElevatorState state = ElevatorState.UNKNOWN; - private static final double kP = 0.2; //0.015 | 0.2 + private static final double kP = 0.2; // 0.015 | 0.2 private static final double kI = 0; private static final double kD = 0; - private static final double kF = 0.35; //0.35 + private static final double kF = 0.35; // 0.35 private static final int MOTION_MAGIC_ACCELERATION = 9001; private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; -// private static final int MOTION_MAGIC_ACCELERATION = 9001; -// private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; -// private static final int MOTION_MAGIC_ACCELERATION = 20594; -// private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; - - private static final double MAX_SCALE_HEIGHT = (12 * 5.5) + 4.6667; + // private static final int MOTION_MAGIC_ACCELERATION = 9001; + // private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; + // private static final int MOTION_MAGIC_ACCELERATION = 20594; + // private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; + + private static final double MAX_SCALE_HEIGHT = 77000;//78900 public static double elevatorHeight = 0; private static Elevator instance; @@ -57,63 +59,90 @@ public Elevator() { elevatorMaster.config_kI(0, kI, Constants.TIMEOUT_MS); elevatorMaster.config_kD(0, kD, Constants.TIMEOUT_MS); elevatorMaster.config_kF(0, kF, Constants.TIMEOUT_MS); + + elevatorMaster.configForwardSoftLimitThreshold((int)MAX_SCALE_HEIGHT, Constants.TIMEOUT_MS); + elevatorMaster.configForwardSoftLimitEnable(true, Constants.TIMEOUT_MS); + elevatorMaster.configReverseSoftLimitThreshold(0, Constants.TIMEOUT_MS); + elevatorMaster.configReverseSoftLimitEnable(true, Constants.TIMEOUT_MS); elevatorMaster.configMotionAcceleration(MOTION_MAGIC_ACCELERATION, Constants.TIMEOUT_MS); elevatorMaster.configMotionCruiseVelocity(MOTION_MAGIC_CRUISE_VELOCITY, Constants.TIMEOUT_MS); elevatorMaster.overrideLimitSwitchesEnable(false); elevatorMaster.configContinuousCurrentLimit(200, Constants.TIMEOUT_MS); - + elevatorMaster.configNominalOutputForward(0, Constants.TIMEOUT_MS); elevatorMaster.configNominalOutputReverse(0, Constants.TIMEOUT_MS); RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(0, Constants.TIMEOUT_MS); RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(0, Constants.TIMEOUT_MS); } - public void setRaw(double speed) { elevatorMaster.set(ControlMode.PercentOutput, speed); } - + public boolean isElevatorAtSetpoint() { return elevatorMaster.getClosedLoopError(0) < 100; } - public void setPosition(double inches){ -// elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR)); - elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR)); + public void setPosition(double inches) { + // elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR)); +// elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR)); + elevatorMaster.set(ControlMode.MotionMagic, inches); elevatorHeight = inches; } - - public void setPosition(ElevatorState state){ - switch(state) { + + public void setPosition(ElevatorState state) { + this.state = state; + // switch(state) { + // case TOP: + // setPosition((12 * 4.5) + 4.667); + // break; + // case SCALE_MAX: + // setPosition(12 * 4.5); + // break; + // case SCALE_MED: + // setPosition(12 * 4); + // break; + // case SCALE_LOW: + // setPosition(12 * 3.5); + // break; + // case BOTTOM: + // setPosition(0); + // break; + // case SWITCH: + // setPosition(12 * 2); + // break; + // } + switch (state) { case TOP: - setPosition((12 * 4.5) + 4.667); + setPosition(MAX_SCALE_HEIGHT); break; case SCALE_MAX: - setPosition(12 * 4.5); + setPosition(77428); break; case SCALE_MED: - setPosition(12 * 4); + setPosition(70000); //66301 break; case SCALE_LOW: - setPosition(12 * 3.5); + setPosition(54297); break; case BOTTOM: setPosition(0); break; case SWITCH: - setPosition(12 * 2); + setPosition(40000); break; } } + public double getElevatorHeightPercentage() { return elevatorHeight / MAX_SCALE_HEIGHT; } public void zero() { elevatorMaster.setSelectedSensorPosition(0, Constants.PID_ID_X, Constants.TIMEOUT_MS); -// setPosition(ElevatorState.BOTTOM); + // setPosition(ElevatorState.BOTTOM); } public boolean getLimitSwitch() { diff --git a/src/com/team3925/frc2018/subsystems/Intake.java b/src/com/team3925/frc2018/subsystems/Intake.java index cc74fd5..a8f0cb2 100644 --- a/src/com/team3925/frc2018/subsystems/Intake.java +++ b/src/com/team3925/frc2018/subsystems/Intake.java @@ -45,12 +45,11 @@ private Intake() { liftMotor.config_kD(0, 0, Constants.TIMEOUT_MS); liftMotor.config_kF(0, 0.35, Constants.TIMEOUT_MS); - liftMotor.configMotionAcceleration(300, Constants.TIMEOUT_MS); - liftMotor.configMotionCruiseVelocity(500, Constants.TIMEOUT_MS); + liftMotor.configMotionAcceleration(400, Constants.TIMEOUT_MS); + liftMotor.configMotionCruiseVelocity(600, Constants.TIMEOUT_MS); liftMotor.setInverted(true); liftMotor.setSensorPhase(true); liftMotor.overrideLimitSwitchesEnable(true); - liftMotor.configContinuousCurrentLimit(10, Constants.TIMEOUT_MS) leftIntake.setInverted(true); } @@ -60,7 +59,7 @@ private void setPosition(double revolutions) { public void setIntakeRollers(double speed) { if (Math.abs(speed) <= 0) - speed = 0.3; + speed = 0.25; leftIntake.set(ControlMode.PercentOutput, speed); rightIntake.set(ControlMode.PercentOutput, speed); } diff --git a/src/com/team3925/utils/MotionProfileCommand.java b/src/com/team3925/utils/MotionProfileCommand.java index fe2f48f..6224e91 100644 --- a/src/com/team3925/utils/MotionProfileCommand.java +++ b/src/com/team3925/utils/MotionProfileCommand.java @@ -1,126 +1,126 @@ -package com.team3925.utils; - -import java.io.File; - -import com.team3925.frc2018.subsystems.Drivetrain; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import jaci.pathfinder.Pathfinder; -import jaci.pathfinder.Trajectory; -import jaci.pathfinder.Waypoint; -import jaci.pathfinder.modifiers.TankModifier; - -/** - * - */ -public class MotionProfileCommand extends Command { - - TankModifier modifier; - - Trajectory trajectory; - Trajectory leftTra; - Trajectory rightTra; - - GnarlyController left; - GnarlyController right; - - Waypoint[] points; - - // TUNE THESE VALUES - public static double kP = 0.3; // 0.5 - public static double kV = 0.5; - public static double kA = 0.2; // 0.07 - public static double kG = 0.3; // 0.5 - - public static final double MAX_VEL = 12; - public static final double MAX_ACL = 24; - public static final double MAX_JERK = 200; - - public static final double WHEEL_BASE_WIDTH = (8.07 / Math.PI); - public static final int ENC_TPR = 1024; - public static final double WHEEL_DI = 0.505; - - public MotionProfileCommand(Waypoint[] points) { - Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, - Trajectory.Config.SAMPLES_HIGH, 0.05, MAX_VEL, MAX_ACL, MAX_JERK); - this.points = points; - trajectory = Pathfinder.generate(this.points, config); - modifier = new TankModifier(trajectory).modify(WHEEL_BASE_WIDTH); - - leftTra = modifier.getLeftTrajectory(); - rightTra = modifier.getRightTrajectory(); - requires(Drivetrain.getInstance()); - - } - - public MotionProfileCommand(String fileName) { - leftTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_left_detailed.csv")); - rightTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_right_detailed.csv")); - requires(Drivetrain.getInstance()); - } - - // Called just before this Command runs the first time - protected void initialize() { - - left = new GnarlyController(leftTra); - right = new GnarlyController(rightTra); - left.config((int) Drivetrain.getInstance().getLeftEncoderPosition(), ENC_TPR, WHEEL_DI, true, - Drivetrain.getInstance().getGyroHeading()); - right.config((int) Drivetrain.getInstance().getRightEncoderPosition(), ENC_TPR, WHEEL_DI, false, - Drivetrain.getInstance().getGyroHeading()); - - left.configurePIDVAG(kP, 0, 0, kV, kA, kG); - right.configurePIDVAG(kP, 0, 0, kV, kA, kG); - - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Drivetrain.getInstance().setVelocity( - left.calculate((int) Drivetrain.getInstance().getLeftEncoderPosition(), - Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getLeftSpeed()), - right.calculate((int) Drivetrain.getInstance().getRightEncoderPosition(), - Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getRightSpeed())); - logData(); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - if (left.isFinished() && right.isFinished()) { - System.out.println("Done"); - return true; - } else { - return false; - } - } - - // Called once after isFinished returns true - protected void end() { - Drivetrain.getInstance().setRaw(0, 0); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - this.end(); - } - - public void logData() { - SmartDashboard.putNumber("Left Position Predicted", left.logPosition()[0]); - SmartDashboard.putNumber("Left Position Actual", left.logPosition()[1]); - - SmartDashboard.putNumber("Left Velocity Predicted", left.logVelocity()[0]); - SmartDashboard.putNumber("Left Velocity Actual", left.logVelocity()[1]); - - SmartDashboard.putNumber("Right Position Predicted", right.logPosition()[0]); - SmartDashboard.putNumber("Right Position Actual", right.logPosition()[1]); - - SmartDashboard.putNumber("Right Velocity Predicted", right.logVelocity()[0]); - SmartDashboard.putNumber("Right Velocity Actual", right.logVelocity()[1]); - - SmartDashboard.putNumber("Heading Predicted", right.logAngle()[0]); - SmartDashboard.putNumber("Heading Actual", right.logAngle()[1]); - } - -} +package com.team3925.utils; + +import java.io.File; + +import com.team3925.frc2018.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import jaci.pathfinder.Waypoint; +import jaci.pathfinder.modifiers.TankModifier; + +/** + * + */ +public class MotionProfileCommand extends Command { + + TankModifier modifier; + + Trajectory trajectory; + Trajectory leftTra; + Trajectory rightTra; + + GnarlyController left; + GnarlyController right; + + Waypoint[] points; + + // TUNE THESE VALUES + public static double kP = 0.3; // 0.5 + public static double kV = 0.5; + public static double kA = 0.3; // 0.07 + public static double kG = 0.5; // 0.5 + + public static final double MAX_VEL = 12; + public static final double MAX_ACL = 24; + public static final double MAX_JERK = 200; + + public static final double WHEEL_BASE_WIDTH = (8.07 / Math.PI); + public static final int ENC_TPR = 1024; + public static final double WHEEL_DI = 0.505; + + public MotionProfileCommand(Waypoint[] points) { + Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, + Trajectory.Config.SAMPLES_HIGH, 0.05, MAX_VEL, MAX_ACL, MAX_JERK); + this.points = points; + trajectory = Pathfinder.generate(this.points, config); + modifier = new TankModifier(trajectory).modify(WHEEL_BASE_WIDTH); + + leftTra = modifier.getLeftTrajectory(); + rightTra = modifier.getRightTrajectory(); + requires(Drivetrain.getInstance()); + + } + + public MotionProfileCommand(String fileName) { + leftTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_left_detailed.csv")); + rightTra = Pathfinder.readFromCSV(new File("/home/lvuser/autos/" + fileName + "_right_detailed.csv")); + requires(Drivetrain.getInstance()); + } + + // Called just before this Command runs the first time + protected void initialize() { + + left = new GnarlyController(leftTra); + right = new GnarlyController(rightTra); + left.config((int) Drivetrain.getInstance().getLeftEncoderPosition(), ENC_TPR, WHEEL_DI, true, + Drivetrain.getInstance().getGyroHeading()); + right.config((int) Drivetrain.getInstance().getRightEncoderPosition(), ENC_TPR, WHEEL_DI, false, + Drivetrain.getInstance().getGyroHeading()); + + left.configurePIDVAG(kP, 0, 0, kV, kA, kG); + right.configurePIDVAG(kP, 0, 0, kV, kA, kG); + + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Drivetrain.getInstance().setVelocity( + left.calculate((int) Drivetrain.getInstance().getLeftEncoderPosition(), + Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getLeftSpeed()), + right.calculate((int) Drivetrain.getInstance().getRightEncoderPosition(), + Drivetrain.getInstance().getGyroHeading(), Drivetrain.getInstance().getRightSpeed())); + logData(); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + if (left.isFinished() && right.isFinished()) { + System.out.println("Done"); + return true; + } else { + return false; + } + } + + // Called once after isFinished returns true + protected void end() { + Drivetrain.getInstance().setRaw(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + this.end(); + } + + public void logData() { + SmartDashboard.putNumber("Left Position Predicted", left.logPosition()[0]); + SmartDashboard.putNumber("Left Position Actual", left.logPosition()[1]); + + SmartDashboard.putNumber("Left Velocity Predicted", left.logVelocity()[0]); + SmartDashboard.putNumber("Left Velocity Actual", left.logVelocity()[1]); + + SmartDashboard.putNumber("Right Position Predicted", right.logPosition()[0]); + SmartDashboard.putNumber("Right Position Actual", right.logPosition()[1]); + + SmartDashboard.putNumber("Right Velocity Predicted", right.logVelocity()[0]); + SmartDashboard.putNumber("Right Velocity Actual", right.logVelocity()[1]); + + SmartDashboard.putNumber("Heading Predicted", right.logAngle()[0]); + SmartDashboard.putNumber("Heading Actual", right.logAngle()[1]); + } + +}