diff --git a/.gitignore b/.gitignore index ef7ea71a..f57c1c77 100644 --- a/.gitignore +++ b/.gitignore @@ -23,3 +23,11 @@ out/ robot2017.iml robot2017.ipr robot2017.iws + +# R workspace files +*.RData +*.Rhistory + +#Other +.metadata/ +.DS_Store diff --git a/.travis.yml b/.travis.yml index 9181e842..c2440bcb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,5 +1,6 @@ sudo: required language: java +jdk: oraclejdk8 script: - ./gradlew build diff --git a/src/main/java/org/usfirst/frc/team449/robot/Robot.java b/src/main/java/org/usfirst/frc/team449/robot/Robot.java index b72e31b6..ceb1fa1e 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/Robot.java +++ b/src/main/java/org/usfirst/frc/team449/robot/Robot.java @@ -1,32 +1,47 @@ package org.usfirst.frc.team449.robot; -import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import maps.org.usfirst.frc.team449.robot.Robot2017Map; +import maps.org.usfirst.frc.team449.robot.components.MotionProfileMap; +import org.usfirst.frc.team449.robot.components.MappedDigitalInput; +import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.DefaultArcadeDrive; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.ExecuteProfile; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.PIDTest; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.SwitchToLowGear; +import org.usfirst.frc.team449.robot.drive.talonCluster.commands.*; +import org.usfirst.frc.team449.robot.drive.talonCluster.util.MPLoader; +import org.usfirst.frc.team449.robot.drive.talonCluster.util.MotionProfileData; +import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; +import org.usfirst.frc.team449.robot.mechanism.activegear.commands.FirePiston; import org.usfirst.frc.team449.robot.mechanism.climber.ClimberSubsystem; import org.usfirst.frc.team449.robot.mechanism.feeder.FeederSubsystem; import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.Intake2017; import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.commands.updown.IntakeUp; -import org.usfirst.frc.team449.robot.mechanism.intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.mechanism.pneumatics.PneumaticsSubsystem; import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.SingleFlywheelShooter; +import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.commands.AccelerateFlywheel; +import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.FireShooter; import org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepad; import org.usfirst.frc.team449.robot.vision.CameraSubsystem; import java.io.IOException; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; /** * The main class of the robot, constructs all the subsystems and initializes default commands. */ public class Robot extends IterativeRobot { + public static double WHEEL_DIAMETER; + + private static final double MP_UPDATE_RATE = 0.005; + + private Notifier MPNotifier; + + /** * The shooter subsystem (flywheel only) */ @@ -67,11 +82,32 @@ public class Robot extends IterativeRobot { */ public static FeederSubsystem feederSubsystem; + public static ActiveGearSubsystem gearSubsystem; + /** * The object constructed directly from map.cfg. * */ private static Robot2017Map.Robot2017 cfg; + public static boolean commandFinished = false; + + private int completedCommands = 0; + + private long startedGearPush = 0; + + private long timeToPushGear; + + private Map rightProfiles, leftProfiles; + + private List talons; + + private boolean redAlliance, dropGear; + private String allianceString; + + private String position; + + private I2C robotInfo; + /** * The method that runs when the robot is turned on. Initializes all subsystems from the map. */ @@ -80,8 +116,7 @@ public void robotInit() { try { //Try to construct map from the cfg file - //cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/balbasaur_map.cfg", - //cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/final_map.cfg", +// cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/balbasaur_map.cfg", cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/fancy_map.cfg", Robot2017Map.Robot2017.newBuilder()); } catch (IOException e) { @@ -90,6 +125,10 @@ public void robotInit() { e.printStackTrace(); } + if (cfg.hasRioduinoPort()) { + robotInfo = new I2C(I2C.Port.kOnboard, cfg.getRioduinoPort()); + } + //Construct the OI (has to be done first because other subsystems take the OI as an argument.) oiSubsystem = new OI2017ArcadeGamepad(cfg.getArcadeOi()); System.out.println("Constructed OI"); @@ -130,15 +169,74 @@ public void robotInit() { feederSubsystem = new FeederSubsystem(cfg.getFeeder()); } + if(cfg.hasGear()){ + gearSubsystem = new ActiveGearSubsystem(cfg.getGear()); + } + + if(cfg.hasBlueRed()){ + redAlliance = new MappedDigitalInput(cfg.getBlueRed()).getStatus().get(0); + if(redAlliance){ + allianceString = "red"; + } else { + allianceString = "blue"; + } + dropGear = new MappedDigitalInput(cfg.getDropGear()).getStatus().get(0); + List tmp = new MappedDigitalInput(cfg.getLocation()).getStatus(); + if (!tmp.get(0) && !tmp.get(1)){ + position = "center"; + } else if (tmp.get(0)){ + position = "left"; + } else { + position = "right"; + } + } + + System.out.println("redAlliance: "+redAlliance); + System.out.println("dropGear: "+dropGear); + System.out.println("positon: "+position); + //Map the buttons (has to be done last because all the subsystems need to have been instantiated.) oiSubsystem.mapButtons(); System.out.println("Mapped buttons"); //Activate the compressor if its module number is in the map. if (cfg.hasModule()) { + System.out.println("Setting up a compressor at module number "+cfg.getModule()); Compressor compressor = new Compressor(cfg.getModule()); compressor.setClosedLoopControl(true); compressor.start(); + System.out.println(compressor.enabled()); + } + + if(cfg.getDoMP()) { + WHEEL_DIAMETER = cfg.getWheelDiameterInches() / 12.; + timeToPushGear = (long) (cfg.getTimeToPushGear() * 1000); + + leftProfiles = new HashMap<>(); + rightProfiles = new HashMap<>(); + + for (MotionProfileMap.MotionProfile profile : cfg.getLeftMotionProfileList()) { + leftProfiles.put(profile.getName(), new MotionProfileData("/home/lvuser/449_resources/" + profile.getFilename(), profile.getInverted())); + } + + for (MotionProfileMap.MotionProfile profile : cfg.getRightMotionProfileList()) { + rightProfiles.put(profile.getName(), new MotionProfileData("/home/lvuser/449_resources/" + profile.getFilename(), profile.getInverted())); + } + + if(cfg.getTestMP()){ + MPLoader.loadTopLevel(leftProfiles.get("test"), driveSubsystem.leftMaster, WHEEL_DIAMETER); + MPLoader.loadTopLevel(rightProfiles.get("test"), driveSubsystem.rightMaster, WHEEL_DIAMETER); + } else { + MPLoader.loadTopLevel(leftProfiles.get(allianceString+"_"+position), driveSubsystem.leftMaster, WHEEL_DIAMETER); + MPLoader.loadTopLevel(rightProfiles.get(allianceString+"_"+position), driveSubsystem.rightMaster, WHEEL_DIAMETER); + } + + talons = new ArrayList<>(); + talons.add(driveSubsystem.leftMaster); + talons.add(driveSubsystem.rightMaster); + MPNotifier = MPLoader.startLoadBottomLevel(talons, MP_UPDATE_RATE); + commandFinished = false; + completedCommands = 0; } } @@ -148,21 +246,54 @@ public void robotInit() { @Override public void teleopInit() { //Stop the drive for safety reasons + if (MPNotifier != null) { + MPNotifier.stop(); + } driveSubsystem.setVBusThrottle(0, 0); + driveSubsystem.overrideNavX = !cfg.getArcadeOi().getOverrideNavXWhileHeld(); driveSubsystem.leftMaster.canTalon.enable(); driveSubsystem.rightMaster.canTalon.enable(); + driveSubsystem.setDefaultCommandManual(new DefaultArcadeDrive(driveSubsystem.straightPID, driveSubsystem, oiSubsystem)); + +// Scheduler.getInstance().add(new PIDTest(driveSubsystem)); //Switch to low gear if we have gears if (driveSubsystem.shifter != null) { - Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem)); + if (cfg.getStartLowGear()){ + Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem)); + } else { + Scheduler.getInstance().add(new SwitchToHighGear(driveSubsystem)); + } } if (intakeSubsystem != null) { Scheduler.getInstance().add(new IntakeUp(intakeSubsystem)); } -// Scheduler.getInstance().add(new DefaultArcadeDrive(driveSubsystem.straightPID, driveSubsystem, oiSubsystem)); + if(gearSubsystem != null){ + Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kForward)); + } + + if (robotInfo != null) { + if (redAlliance) { + String WriteString = "red_teleop"; + char[] CharArray = WriteString.toCharArray(); + byte[] WriteData = new byte[CharArray.length]; + for (int i = 0; i < CharArray.length; i++) { + WriteData[i] = (byte) CharArray[i]; + } + robotInfo.transaction(WriteData, WriteData.length, null, 0); + } else { + String WriteString = "blue_teleop"; + char[] CharArray = WriteString.toCharArray(); + byte[] WriteData = new byte[CharArray.length]; + for (int i = 0; i < CharArray.length; i++) { + WriteData[i] = (byte) CharArray[i]; + } + robotInfo.transaction(WriteData, WriteData.length, null, 0); + } + } } /** @@ -171,6 +302,15 @@ public void teleopInit() { @Override public void teleopPeriodic() { //Run all commands. This is a WPILib thing you don't really have to worry about. + if (singleFlywheelShooterSubsystem != null) { + SmartDashboard.putBoolean("Shooter Running", singleFlywheelShooterSubsystem.spinning); + SmartDashboard.putNumber("Shooter Speed", singleFlywheelShooterSubsystem.talon.getSpeed()); + SmartDashboard.putNumber("Shooter Error", singleFlywheelShooterSubsystem.talon.getError()); + SmartDashboard.putNumber("Shooter Current", singleFlywheelShooterSubsystem.talon.canTalon.getOutputCurrent()); + } + if(gearSubsystem != null) { + SmartDashboard.putBoolean("Gear Open", gearSubsystem.contracted); + } Scheduler.getInstance().run(); } @@ -180,14 +320,52 @@ public void teleopPeriodic() { @Override public void autonomousInit() { //Set throttle to 0 for safety reasons + //Switch to low gear if we have gears if (driveSubsystem.shifter != null) { - Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem)); + if (cfg.getStartLowGear()){ + Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem)); + } else { + Scheduler.getInstance().add(new SwitchToHighGear(driveSubsystem)); + } } - driveSubsystem.leftMaster.canTalon.enable(); - driveSubsystem.rightMaster.canTalon.enable(); + + if(gearSubsystem != null){ + Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kForward)); + } + + commandFinished = false; + driveSubsystem.setVBusThrottle(0, 0); - Scheduler.getInstance().add(new PIDTest(driveSubsystem)); -// Scheduler.getInstance().add(new ExecuteProfile(driveSubsystem)); + + if (cfg.getDoMP()) { + if (singleFlywheelShooterSubsystem != null && !cfg.getTestMP()){ + Scheduler.getInstance().add(new AccelerateFlywheel(singleFlywheelShooterSubsystem, 20)); + } + Scheduler.getInstance().add(new ExecuteProfile(talons, 15, driveSubsystem)); + + if (robotInfo != null) { + if (redAlliance) { + String WriteString = "red_auto"; + char[] CharArray = WriteString.toCharArray(); + byte[] WriteData = new byte[CharArray.length]; + for (int i = 0; i < CharArray.length; i++) { + WriteData[i] = (byte) CharArray[i]; + } + robotInfo.transaction(WriteData, WriteData.length, null, 0); + } else { + String WriteString = "blue_auto"; + char[] CharArray = WriteString.toCharArray(); + byte[] WriteData = new byte[CharArray.length]; + for (int i = 0; i < CharArray.length; i++) { + WriteData[i] = (byte) CharArray[i]; + } + robotInfo.transaction(WriteData, WriteData.length, null, 0); + } + + } + }else { + Scheduler.getInstance().add(new PIDTest(driveSubsystem, cfg.getDriveBackTime())); + } } /** @@ -197,6 +375,56 @@ public void autonomousInit() { public void autonomousPeriodic() { //Run all commands. This is a WPILib thing you don't really have to worry about. Scheduler.getInstance().run(); - SmartDashboard.putNumber("Heading", driveSubsystem.getGyroOutput()); + if (cfg.getDoMP() && !cfg.getTestMP()) { + if (System.currentTimeMillis() - startedGearPush > timeToPushGear && completedCommands == 1) { + commandFinished = true; + } + if (commandFinished) { + System.out.println("A command finished!"); + completedCommands++; + commandFinished = false; + if (completedCommands == 1) { + if (gearSubsystem != null && dropGear) { + Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kReverse)); + } + startedGearPush = System.currentTimeMillis(); + } else if (completedCommands == 2) { + if (position.equals("center")) { + Scheduler.getInstance().add(new DriveAtSpeed(driveSubsystem, -0.3, cfg.getDriveBackTime())); + } else if (position.equals("right") && redAlliance) { + loadProfile("red_shoot"); + Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem)); + } else if (position.equals("left") && !redAlliance) { + loadProfile("blue_shoot"); + Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem)); + } else if (redAlliance){ + loadProfile("red_backup"); + Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem)); + } else { + loadProfile("blue_backup"); + Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem)); + } + } else if (completedCommands == 3) { + if (((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance)) && singleFlywheelShooterSubsystem != null) { + Scheduler.getInstance().add(new FireShooter(singleFlywheelShooterSubsystem, intakeSubsystem, feederSubsystem)); + } else if (!((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance))) { + loadProfile("forward"); + Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem)); + } + } + } + } + } + + @Override + public void disabledInit(){ + driveSubsystem.setVBusThrottle(0, 0); + } + + private void loadProfile(String name){ + MPNotifier.stop(); + MPLoader.loadTopLevel(leftProfiles.get(name), driveSubsystem.leftMaster, WHEEL_DIAMETER); + MPLoader.loadTopLevel(rightProfiles.get(name), driveSubsystem.rightMaster, WHEEL_DIAMETER); + MPNotifier.startPeriodic(MP_UPDATE_RATE); } -} \ No newline at end of file +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java b/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java new file mode 100644 index 00000000..933ad374 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java @@ -0,0 +1,35 @@ +package org.usfirst.frc.team449.robot.components; + +import edu.wpi.first.wpilibj.DigitalInput; +import maps.org.usfirst.frc.team449.robot.components.DigitalInputMap; + +import java.util.ArrayList; +import java.util.List; + +/** + * Created by bryanli on 3/20/17. + */ +public class MappedDigitalInput { + private DigitalInputMap.DigitalInput map; + + private List digitalInputs; + + + public MappedDigitalInput(DigitalInputMap.DigitalInput map) { + this.map = map; + + digitalInputs = new ArrayList<>(); + for (int portNum : map.getPortList()) { + DigitalInput tmp = new DigitalInput(portNum); + digitalInputs.add(tmp); + } + } + + public List getStatus(){ + List digitalValues = new ArrayList<>(); + for (int i = 0; i < digitalInputs.size(); i++) { + digitalValues.add(!digitalInputs.get(i).get()); + } + return digitalValues; + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java b/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java index 39b10dd6..9d9a48e3 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java +++ b/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java @@ -22,7 +22,7 @@ public class RotPerSecCANTalonSRX extends Component { /** * The counts per rotation of the encoder being used. */ - public double encoderCPR; + public int encoderCPR; /** * The type of encoder the talon uses. @@ -80,6 +80,17 @@ public RotPerSecCANTalonSRX(RotPerSecCANTalonSRXMap.RotPerSecCANTalonSRX map) { //Choose which profile to use, regular driving or MP. canTalon.setProfile(map.getProfile()); + + if (map.hasClosedLoopRampRate()){ + canTalon.setCloseLoopRampRate(map.getClosedLoopRampRate()); + } + + if (map.hasCurrentLimit()){ + canTalon.setCurrentLimit(map.getCurrentLimit()); + canTalon.EnableCurrentLimit(true); + } else { + canTalon.EnableCurrentLimit(false); + } } /** @@ -232,6 +243,10 @@ public double getError() { return nativeToRPS(canTalon.getError()); } + public double getSetpoint(){ + return nativeToRPS(canTalon.getSetpoint()); + } + /** * Get the high gear max speed. Sometimes useful for scaling joystick output. * @@ -240,4 +255,8 @@ public double getError() { public double getMaxSpeedHG() { return map.getMaxSpeedHg(); } + + public double getPower() { + return canTalon.getOutputVoltage()*canTalon.getOutputCurrent(); + } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/components/TriggerButton.java b/src/main/java/org/usfirst/frc/team449/robot/components/TriggerButton.java index c81e1b1c..6cd09bc7 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/components/TriggerButton.java +++ b/src/main/java/org/usfirst/frc/team449/robot/components/TriggerButton.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; +import maps.org.usfirst.frc.team449.robot.oi.JoystickButtonMap; import maps.org.usfirst.frc.team449.robot.oi.TriggerButtonMap; import org.usfirst.frc.team449.robot.oi.components.SmoothedThrottle; import org.usfirst.frc.team449.robot.oi.components.Throttle; @@ -23,6 +24,10 @@ public TriggerButton(TriggerButtonMap.TriggerButton map){ this(map.getPort(), map.getAxis(), map.getTriggerAt()); } + public TriggerButton(JoystickButtonMap.JoystickButton map){ + this(map.getPort(), map.getButtonIndex(), map.getTriggerAt()); + } + @Override public boolean get() { return throttle.getValue() >= triggerAt; diff --git a/src/main/java/org/usfirst/frc/team449/robot/components/dPadButton.java b/src/main/java/org/usfirst/frc/team449/robot/components/dPadButton.java new file mode 100644 index 00000000..f7e50a2f --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/components/dPadButton.java @@ -0,0 +1,28 @@ +package org.usfirst.frc.team449.robot.components; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; +import maps.org.usfirst.frc.team449.robot.oi.JoystickButtonMap; + +/** + * Created by Noah Gleason on 4/6/2017. + */ +public class dPadButton extends Button { + + private int angle; + private Joystick joystick; + + public dPadButton(int port, int angle){ + this.angle = angle; + this.joystick = new Joystick(port); + } + + @Override + public boolean get() { + return joystick.getPOV() == angle; + } + + public dPadButton(JoystickButtonMap.JoystickButton map){ + this(map.getPort(), map.getAngle()); + } +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java index ca1a5b33..b1e9feca 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java @@ -4,14 +4,17 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import maps.org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRXMap; import maps.org.usfirst.frc.team449.robot.components.ToleranceBufferAnglePIDMap; +import org.usfirst.frc.team449.robot.Robot; import org.usfirst.frc.team449.robot.components.NavxSubsystem; import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; import org.usfirst.frc.team449.robot.drive.DriveSubsystem; import org.usfirst.frc.team449.robot.drive.talonCluster.commands.DefaultArcadeDrive; import org.usfirst.frc.team449.robot.drive.talonCluster.commands.ExecuteProfile; +import org.usfirst.frc.team449.robot.drive.talonCluster.util.MPLoader; import org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepad; import java.io.FileWriter; @@ -71,7 +74,7 @@ public class TalonClusterDrive extends DriveSubsystem implements NavxSubsystem { public CANTalon.MotionProfileStatus rightTPointStatus; /** - * The time (nanoseconds) when the robot was enabled (for use in logging) + * The time (milliseconds) when the robot was enabled (for use in logging) */ private long startTime; @@ -80,6 +83,8 @@ public class TalonClusterDrive extends DriveSubsystem implements NavxSubsystem { */ private String logFN; + private String errorFN; + /** * Whether or not to use the NavX for driving straight */ @@ -180,10 +185,17 @@ public TalonClusterDrive(maps.org.usfirst.frc.team449.robot.drive.talonCluster.T // Write the headers for the logfile. // TODO externalize this shit - logFN = "/home/lvuser/logs/driveLog-" + new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss").format(new Date()) + "" + - ".csv"; + String timeStamp = new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss").format(new Date()); + logFN = "/home/lvuser/logs/drivePowerLog-" + timeStamp + ".csv"; + errorFN = "/home/lvuser/logs/driveErrorLog-" + timeStamp + ".csv"; try (PrintWriter writer = new PrintWriter(logFN)) { - writer.println("time,left,right,left error,right error,left setpoint,right setpoint"); + writer.println("time,left vel,right vel,left setpoint,right setpoint,left current,right current,left voltage,right voltage"); + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + try (PrintWriter writer = new PrintWriter(errorFN)) { + writer.println("time,message"); writer.close(); } catch (IOException e) { e.printStackTrace(); @@ -283,34 +295,87 @@ private void setPIDThrottle(double left, double right) { public void setDefaultThrottle(double left, double right) { //Clip to one to avoid anything strange. setPIDThrottle(clipToOne(left), clipToOne(right)); - //setVBusThrottle(left, right); +// setVBusThrottle(left, right); + } + + public void logPower(){ + //time,left vel,right vel,left setpoint,right setpoint,left current,right current,left voltage,right voltage + /* + try (FileWriter fw = new FileWriter(logFN, true)) { + StringBuilder sb = new StringBuilder(); + sb.append((double)(System.currentTimeMillis() - startTime) / 1000.); + sb.append(","); + sb.append(leftMaster.getSpeed()); + sb.append(","); + sb.append(rightMaster.getSpeed()); + sb.append(","); + sb.append(leftMaster.getSetpoint()); + sb.append(","); + sb.append(rightMaster.getSetpoint()); + sb.append(","); + sb.append(leftMaster.canTalon.getOutputCurrent()); + sb.append(","); + sb.append(rightMaster.canTalon.getOutputCurrent()); + sb.append(","); + sb.append(leftMaster.canTalon.getOutputVoltage()); + sb.append(","); + sb.append(rightMaster.canTalon.getOutputVoltage()); + sb.append("\n"); + fw.write(sb.toString()); + fw.close(); + } catch (IOException e) { + e.printStackTrace(); + }*/ + SmartDashboard.putNumber("Left Speed", leftMaster.getSpeed()); + SmartDashboard.putNumber("Left Setpoint", leftMaster.getSetpoint()); + SmartDashboard.putNumber("Left Current", leftMaster.canTalon.getOutputCurrent()); + SmartDashboard.putNumber("Left Voltage", leftMaster.canTalon.getOutputVoltage()); + SmartDashboard.putNumber("Right Speed", rightMaster.getSpeed()); + SmartDashboard.putNumber("Right Setpoint", rightMaster.getSetpoint()); + SmartDashboard.putNumber("Right Current", rightMaster.canTalon.getOutputCurrent()); + SmartDashboard.putNumber("Right Voltage", rightMaster.canTalon.getOutputVoltage()); + } + + public void logError(String error){ + try (FileWriter fw = new FileWriter(errorFN, true)) { + fw.write(((double)(System.currentTimeMillis() - startTime) / 1000.)+","+error+"\n"); + } catch (IOException e) { + e.printStackTrace(); + } } /** * Log stuff to file */ public void logData() { + logPower(); + /* //Print stuff to the log file for in-depth analysis or tuning. try (FileWriter fw = new FileWriter(logFN, true)) { StringBuilder sb = new StringBuilder(); - sb.append((System.nanoTime() - startTime) / Math.pow(10, 9)); + sb.append((double)(System.currentTimeMillis() - startTime) / 1000.); sb.append(","); sb.append(leftMaster.getSpeed()); sb.append(","); sb.append(rightMaster.getSpeed()); sb.append(","); - sb.append(leftMaster.canTalon.getSpeed()); - sb.append(","); - sb.append(rightMaster.canTalon.getSpeed()); sb.append(leftMaster.getError()); sb.append(","); sb.append(rightMaster.getError()); + sb.append(","); + sb.append(leftMaster.canTalon.getOutputCurrent()); + sb.append(","); + sb.append(rightMaster.canTalon.getOutputCurrent()); + sb.append(","); + sb.append(leftMaster.canTalon.getOutputVoltage()); + sb.append(","); + sb.append(rightMaster.canTalon.getOutputVoltage()); /* sb.append(","); sb.append(leftTPointStatus.activePoint.position); sb.append(","); sb.append(rightTPointStatus.activePoint.position); - */ + */ /* sb.append("\n"); fw.write(sb.toString()); @@ -333,6 +398,9 @@ public void logData() { SmartDashboard.putNumber("Left P", leftMaster.canTalon.getP()); SmartDashboard.putNumber("Right P", rightMaster.canTalon.getP()); SmartDashboard.putBoolean("In low gear?", lowGear); + SmartDashboard.putNumber("Right pos", MPLoader.nativeToFeet(rightMaster.canTalon.getEncPosition(), rightMaster.encoderCPR, Robot.WHEEL_DIAMETER)); + SmartDashboard.putNumber("Left pos", MPLoader.nativeToFeet(leftMaster.canTalon.getEncPosition(), leftMaster.encoderCPR, Robot.WHEEL_DIAMETER)); + */ } /** @@ -341,10 +409,12 @@ public void logData() { * @param sp velocity setpoint */ public void logData(double sp) { + logPower(); + /* //Print stuff to the log file for in-depth analysis or tuning. try (FileWriter fw = new FileWriter(logFN, true)) { StringBuilder sb = new StringBuilder(); - sb.append((System.nanoTime() - startTime) / Math.pow(10, 9)); + sb.append((double)(System.currentTimeMillis() - startTime) / 1000.); sb.append(","); sb.append(leftMaster.getSpeed()); sb.append(","); @@ -354,11 +424,9 @@ public void logData(double sp) { sb.append(","); sb.append(rightMaster.getError()); sb.append(","); - sb.append(rightTPointStatus.activePoint.position); - sb.append(","); - sb.append(leftTPointStatus.activePoint.velocity); + sb.append(sp); sb.append(","); - sb.append(rightTPointStatus.activePoint.velocity); + sb.append(sp); sb.append("\n"); fw.write(sb.toString()); @@ -385,7 +453,7 @@ public void logData(double sp) { sb.append(leftTPointStatus.activePoint.position); sb.append(","); sb.append(rightTPointStatus.activePoint.position); - */ + */ /* sb.append("\n"); fw.write(sb.toString()); @@ -408,6 +476,7 @@ public void logData(double sp) { SmartDashboard.putNumber("Left P", leftMaster.canTalon.getP()); SmartDashboard.putNumber("Right P", rightMaster.canTalon.getP()); SmartDashboard.putBoolean("In low gear?", lowGear); + */ } /** @@ -417,11 +486,13 @@ public void logData(double sp) { @Override protected void initDefaultCommand() { //Set the start time to current time - startTime = System.nanoTime(); - //Start overriding the NavX. - overrideNavX = false; + startTime = System.currentTimeMillis(); //Start driving - setDefaultCommand(new DefaultArcadeDrive(straightPID, this, oi)); + //setDefaultCommand(new DefaultArcadeDrive(straightPID, this, oi)); + } + + public void setDefaultCommandManual(Command defaultCommand){ + setDefaultCommand(defaultCommand); } /** @@ -500,9 +571,11 @@ public boolean inLowGear() { */ public boolean shouldDownshift() { //We should shift if we're going slower than the downshift speed - boolean okToShift = Math.min(Math.abs(getLeftSpeed()), Math.abs(getRightSpeed())) < downshift; + boolean okToShift = Math.max(Math.abs(getLeftSpeed()), Math.abs(getRightSpeed())) < downshift; //Or if we're just turning in place. okToShift = okToShift || (oi.getFwd() == 0 && oi.getRot() != 0); + //Or commanding a low speed. + okToShift = okToShift || (Math.abs(oi.getFwd()) < upshiftFwdDeadband); //But there's no need to downshift if we're already in low gear. okToShift = okToShift && !lowGear; //And we don't want to shift if autoshifting is turned off. @@ -531,7 +604,7 @@ public boolean shouldDownshift() { */ public boolean shouldUpshift() { //We should shift if we're going faster than the upshift speed... - boolean okToShift = Math.max(Math.abs(getLeftSpeed()), Math.abs(getRightSpeed())) > upshift; + boolean okToShift = Math.min(Math.abs(getLeftSpeed()), Math.abs(getRightSpeed())) > upshift; //AND the driver's trying to go forward fast. okToShift = okToShift && Math.abs(oi.getFwd()) > upshiftFwdDeadband; //But there's no need to upshift if we're already in high gear. diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DefaultArcadeDrive.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DefaultArcadeDrive.java index 250fc21c..cf470570 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DefaultArcadeDrive.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DefaultArcadeDrive.java @@ -39,6 +39,13 @@ public class DefaultArcadeDrive extends PIDAngleCommand { * The maximum velocity for the robot to be at in order to switch to driveStraight, in degrees/sec */ private double maxAngularVel; + + private long driveStraightDelay; + + private long timeAbleToDriveStraight; + + private boolean delayedDriveStraight; + /** * The map of values */ @@ -59,6 +66,10 @@ public DefaultArcadeDrive(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map this.map = map; driveSubsystem = drive; + timeAbleToDriveStraight = 0; + driveStraightDelay = (long) (map.getDriveStraightDelay() * 1000); + delayedDriveStraight = false; + //Needs a requires because it's a default command. requires(drive); @@ -98,25 +109,33 @@ protected void execute() { rot = oi.getRot(); //If we're driving straight but the driver tries to turn or overrides the NavX: - if ((drivingStraight && rot != 0) || driveSubsystem.overrideNavX) { + if (drivingStraight && (rot != 0 || driveSubsystem.overrideNavX)) { //Switch to free drive drivingStraight = false; - System.out.println("Switching to free drive."); + delayedDriveStraight = false; + //System.out.println("Switching to free drive."); } //If we're free driving and the driver lets go of the turn stick: - else if (!(drivingStraight) && rot == 0 && Math.abs(driveSubsystem.navx.getRate()) <= maxAngularVel) { + else if (!(driveSubsystem.overrideNavX) && !(delayedDriveStraight) && !(drivingStraight) && rot == 0 && Math.abs(driveSubsystem.navx.getRate()) <= maxAngularVel) { + delayedDriveStraight = true; + timeAbleToDriveStraight = System.currentTimeMillis(); + } + + if(delayedDriveStraight && System.currentTimeMillis() - timeAbleToDriveStraight >= driveStraightDelay){ //Switch to driving straight drivingStraight = true; + delayedDriveStraight = false; //Set the setpoint to the current heading and reset the NavX this.getPIDController().reset(); this.getPIDController().setSetpoint(subsystem.getGyroOutput()); this.getPIDController().enable(); - System.out.println("Switching to DriveStraight."); + //System.out.println("Switching to DriveStraight."); } //Log data and stuff driveSubsystem.logData(); SmartDashboard.putBoolean("driving straight?", drivingStraight); + SmartDashboard.putBoolean("Override Navx", driveSubsystem.overrideNavX); SmartDashboard.putNumber("Vel Axis", vel); SmartDashboard.putNumber("Rot axis", rot); } diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DriveAtSpeed.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DriveAtSpeed.java index 2f1a577a..0a543d00 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DriveAtSpeed.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/DriveAtSpeed.java @@ -46,6 +46,7 @@ protected void initialize() { startTime = System.nanoTime(); //Reset drive speed (for safety reasons) ((TalonClusterDrive) subsystem).setDefaultThrottle(0.0, 0.0); + System.out.println("DriveAtSpeed init"); } /** @@ -77,6 +78,7 @@ protected void end() { ((TalonClusterDrive) subsystem).logData(); //Brake on exit. ((TalonClusterDrive) subsystem).setDefaultThrottle(0, 0); + System.out.println("DriveAtSpeed end."); } /** diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ExecuteProfile.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ExecuteProfile.java index d0f0b91d..de99ea97 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ExecuteProfile.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ExecuteProfile.java @@ -1,92 +1,57 @@ package org.usfirst.frc.team449.robot.drive.talonCluster.commands; import com.ctre.CANTalon; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import maps.org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDriveMap; -import org.usfirst.frc.team449.robot.ReferencingCommand; -import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; -import org.usfirst.frc.team449.robot.drive.talonCluster.util.MPUpdaterProcess; -import org.usfirst.frc.team449.robot.drive.talonCluster.util.MotionProfileData; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Subsystem; +import org.usfirst.frc.team449.robot.Robot; +import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; + +import java.util.ArrayList; +import java.util.Collection; /** * ReferencingCommand to load and execute a motion profile on the master Talons in the two motor clusters */ -public class ExecuteProfile extends ReferencingCommand { - private static final double WHEEL_DIAMETER = 4; //In inches +public class ExecuteProfile extends Command { //TODO Externalize all this shit /** * Number of points that must be loaded to the bottom level buffer before we start executing the profile */ - private static final int MIN_NUM_POINTS_IN_BTM = 128; // maximum number of points + private static final int MIN_NUM_POINTS_IN_BTM = 10; // maximum number of points - /** - * Filenames of the MP input files - */ - private static final String LEFT_IN_FILE_NAME = "/home/lvuser/449_resources/leftProfile.csv"; - private static final String RIGHT_IN_FILE_NAME = "/home/lvuser/449_resources/rightProfile.csv"; + private boolean bottomLoaded; - /** - * Update rate of the {@link MPUpdaterProcess} - */ - private static final double UPDATE_RATE = 0.005; // MP processing thread update rate copied from CTRE example + private Collection talons; - // TODO make _state and enum - /** - * State variable of the FSM used to chose when to load points and when to execute the profile - */ - private int _state = 0; - - /** - * Scheduler {@link Notifier} for updating the Talons' MP - */ - private Notifier mpProcessNotifier; + private boolean finished; - /** - * Drive that the MP will be executed on - */ - private TalonClusterDrive tcd; + private long timeout; - /** - * Motion profile data - * - * @deprecated to be replaced with PathGenerator stuff in MP_2_Sides future - */ - private MotionProfileData leftProfile; - private MotionProfileData rightProfile; + private long startTime; - /** - * MP status of the left master - */ - private CANTalon.MotionProfileStatus leftStatus; - - /** - * MP status of the right master - */ - private CANTalon.MotionProfileStatus rightStatus; - - private boolean finished; /** * Construct a new ExecuteProfile command - * - * @param subsystem drive subsystem to execute this command on */ - public ExecuteProfile(TalonClusterDrive subsystem) { - super(subsystem); - requires(subsystem); + public ExecuteProfile(Collection talons, double timeout, Subsystem toRequire) { + if (toRequire != null){ + requires(toRequire); + } - tcd = subsystem; + this.talons = new ArrayList<>(); - leftStatus = new CANTalon.MotionProfileStatus(); - rightStatus = new CANTalon.MotionProfileStatus(); + for (RotPerSecCANTalonSRX talon : talons){ + this.talons.add(talon.canTalon); + } - leftProfile = new MotionProfileData(LEFT_IN_FILE_NAME); - rightProfile = new MotionProfileData(RIGHT_IN_FILE_NAME); + this.timeout = (long) (timeout * 1000); finished = false; + bottomLoaded = false; + } - mpProcessNotifier = null; // WARNING not assigned until after "initialize" is called + public ExecuteProfile(Collection talons, double timeout){ + this(talons, timeout, null); } /** @@ -94,18 +59,16 @@ public ExecuteProfile(TalonClusterDrive subsystem) { */ @Override protected void initialize() { - // Put the masters in motion profile mode - tcd.leftMaster.canTalon.changeControlMode(CANTalon.TalonControlMode.MotionProfile); - tcd.rightMaster.canTalon.changeControlMode(CANTalon.TalonControlMode.MotionProfile); - - // Make sure they are disabled while they have data piped into them - tcd.leftMaster.canTalon.set(CANTalon.SetValueMotionProfile.Disable.value); - tcd.rightMaster.canTalon.set(CANTalon.SetValueMotionProfile.Disable.value); + for (CANTalon talon : talons) { + talon.changeControlMode(CANTalon.TalonControlMode.MotionProfile); + talon.set(CANTalon.SetValueMotionProfile.Disable.value); + talon.clearMotionProfileHasUnderrun(); + } - tcd.leftMaster.canTalon.clearMotionProfileHasUnderrun(); - tcd.rightMaster.canTalon.clearMotionProfileHasUnderrun(); + startTime = System.currentTimeMillis(); finished = false; + bottomLoaded = false; } /** @@ -114,175 +77,51 @@ protected void initialize() { */ @Override protected void execute() { - control(); - /* - SmartDashboard.putNumber("Left MP Error", tcd.leftMaster.getSpeed() - tcd.leftMaster.nativeToRPS(leftStatus.activePoint.velocity)); - SmartDashboard.putNumber("Right MP Error", tcd.rightMaster.getSpeed() - tcd.rightMaster.nativeToRPS(rightStatus.activePoint.velocity)); - System.out.println("Active Point: " + pointToString(leftStatus.activePoint)); - System.out.println("Output Enable: " + leftStatus.outputEnable + ", " + rightStatus.outputEnable); - if (!leftStatus.activePointValid) { - System.out.println("INVALID! YOU DONE FUCKED UP LEFT SIDE"); - System.out.println("Left active point: " + pointToString(leftStatus.activePoint)); - } - if (!rightStatus.activePointValid) { - System.out.println("INVALID! YOU DONE FUCKED UP RIGHT SIDE"); - System.out.println("Right active point: " + pointToString(rightStatus.activePoint)); - } - if (leftStatus.activePoint.isLastPoint || rightStatus.activePoint.isLastPoint) { - System.out.println("LAST POINT"); + finished = true; + boolean bottomNowLoaded = true; + for (CANTalon talon : talons) { + CANTalon.MotionProfileStatus MPStatus = new CANTalon.MotionProfileStatus(); + talon.getMotionProfileStatus(MPStatus); + if (!bottomLoaded) { + finished = false; + bottomNowLoaded = bottomNowLoaded && (MPStatus.btmBufferCnt >= MIN_NUM_POINTS_IN_BTM); + } + if (bottomLoaded) { + finished = finished && MPStatus.activePoint.isLastPoint; + } } - if (leftStatus.activePoint.zeroPos || rightStatus.activePoint.zeroPos) { - System.out.println("FIRST POINT"); + if (bottomNowLoaded && !bottomLoaded) { + bottomLoaded = true; + System.out.println("Enabling the talons!"); + for (CANTalon talon : talons) { + talon.enable(); + talon.set(CANTalon.SetValueMotionProfile.Enable.value); + } } - */ - tcd.logData(); + Robot.driveSubsystem.logData(); } @Override - protected boolean isFinished(){ - return finished; + protected boolean isFinished() { + return finished || (System.currentTimeMillis() - startTime > timeout); } @Override - protected void end(){ - tcd.leftMaster.canTalon.set(CANTalon.SetValueMotionProfile.Hold.value); - tcd.rightMaster.canTalon.set(CANTalon.SetValueMotionProfile.Hold.value); - mpProcessNotifier.stop(); + protected void end() { + for (CANTalon talon : talons) { + talon.set(CANTalon.SetValueMotionProfile.Hold.value); +// talon.disable(); + } + Robot.commandFinished = true; System.out.println("ExecuteProfile end."); } @Override - protected void interrupted(){ - mpProcessNotifier.stop(); - tcd.leftMaster.canTalon.set(CANTalon.SetValueMotionProfile.Disable.value); - tcd.rightMaster.canTalon.set(CANTalon.SetValueMotionProfile.Disable.value); - System.out.println("ExecuteProfile interrupted!"); - } - - /** - * Execute the logic that controls if trajectory points are being loaded or if the profile is being executed - */ - private void control() { - tcd.leftMaster.canTalon.getMotionProfileStatus(leftStatus); - tcd.rightMaster.canTalon.getMotionProfileStatus(rightStatus); - - if (leftStatus.isUnderrun || rightStatus.isUnderrun) - System.out.println("UNDERRUN! That's BAAAD!"); - - switch (_state) { - case 0: -// System.out.println("State 0."); - startFilling(); - _state = 1; - break; - case 1: -// System.out.println("State 1"); - mpProcessNotifier.startPeriodic(UPDATE_RATE); - tcd.leftMaster.canTalon.changeMotionControlFramePeriod((int) (UPDATE_RATE * 1e3)); // TODO figure out what this does - tcd.rightMaster.canTalon.changeMotionControlFramePeriod((int) (UPDATE_RATE * 1e3)); - System.out.println("LEFT BTM BUFF CNT " + leftStatus.btmBufferCnt); - System.out.println("RIGHT BTM BUFF CNT " + rightStatus.btmBufferCnt); - - if (leftStatus.btmBufferCnt >= MIN_NUM_POINTS_IN_BTM && rightStatus.btmBufferCnt >= MIN_NUM_POINTS_IN_BTM) { - _state = 2; - System.out.println("LOADED"); - } else { - System.out.println("NOT FULLY LOADED"); - } - break; - case 2: -// System.out.println("State 2"); - tcd.leftMaster.canTalon.changeControlMode(CANTalon.TalonControlMode.MotionProfile); - tcd.rightMaster.canTalon.changeControlMode(CANTalon.TalonControlMode.MotionProfile); - tcd.leftMaster.canTalon.enable(); - tcd.rightMaster.canTalon.enable(); - tcd.leftMaster.canTalon.set(CANTalon.SetValueMotionProfile.Enable.value); - tcd.rightMaster.canTalon.set(CANTalon.SetValueMotionProfile.Enable.value); - tcd.leftMaster.canTalon.getMotionProfileStatus(leftStatus); - tcd.rightMaster.canTalon.getMotionProfileStatus(rightStatus); - if(leftStatus.outputEnable == CANTalon.SetValueMotionProfile.Enable && rightStatus.outputEnable == CANTalon.SetValueMotionProfile.Enable) - _state = 3; - break; - case 3: -// System.out.println("State 3"); - if(leftStatus.activePoint.isLastPoint && rightStatus.activePoint.isLastPoint){ - finished = true; - } - break; - default: - System.out.println("Default state, something went wrong."); - break; - } - } - - /** - * Fill the Talon MP hardware buffer - */ - private void startFilling() { - MPUpdaterProcess updaterProcess = new MPUpdaterProcess(); - - tcd.rightMaster.canTalon.clearMotionProfileTrajectories(); - tcd.leftMaster.canTalon.clearMotionProfileTrajectories(); - - // Fill the Talon's buffer with points - CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint(); - for (int i = 0; i < leftProfile.data.length; ++i) { - // Set all the fields of the profile point - point.position = -inchesToNative(leftProfile.data[i][0]); - point.velocity = -tcd.leftMaster.RPStoNative(leftProfile.data[i][1]); - point.timeDurMs = (int) (leftProfile.data[i][2] * 1000.); - point.profileSlotSelect = 1; // gain selection - point.velocityOnly = false; // true => no position servo just velocity feedforward - point.zeroPos = i == 0; // If its the first point, zeroPos = true - point.isLastPoint = (i + 1) == leftProfile.data.length; // If its the last point, isLastPoint = true - - // Send the point to the Talon's buffer - if(!tcd.leftMaster.canTalon.pushMotionProfileTrajectory(point)) { - System.out.println("Left buffer full!"); - break; - } - -// System.out.println("LEFT POINT "+(i+1)+": "+pointToString(point)); - } - - for (int i = 0; i < rightProfile.data.length; ++i) { - // Set all the fields of the profile point - point.position = -inchesToNative(rightProfile.data[i][0]) * ((TalonClusterDriveMap.TalonClusterDrive) tcd.map).getL2R(); - point.velocity = -tcd.leftMaster.RPStoNative(rightProfile.data[i][1]) * ((TalonClusterDriveMap.TalonClusterDrive) tcd.map).getL2R(); - point.timeDurMs = (int) (rightProfile.data[i][2] * 1000.); - point.profileSlotSelect = 1; // gain selection - point.velocityOnly = false; // true => no position servo just velocity feedforward - point.zeroPos = i == 0; // If its the first point, zeroPos = true - point.isLastPoint = (i + 1) == rightProfile.data.length; // If its the last point, isLastPoint = true - - // Send the point to the Talon's buffer - if (!tcd.rightMaster.canTalon.pushMotionProfileTrajectory(point)){ - System.out.println("Right buffer full!"); - break; - } - -// System.out.println("RIGHT POINT "+(i+1)+": "+pointToString(point)); + protected void interrupted() { + for (CANTalon talon : talons) { + talon.set(CANTalon.SetValueMotionProfile.Disable.value); } - - - // Add the Talons to the updater thread (thread should not have started yet tho) - updaterProcess.addTalon(tcd.leftMaster.canTalon); - updaterProcess.addTalon(tcd.rightMaster.canTalon); - mpProcessNotifier = new Notifier(updaterProcess); - System.out.println("Finished loading points"); - } - - private double nativeToInches(double nativeUnits){ - double rotations = nativeUnits / (tcd.leftMaster.encoderCPR*4); - return rotations * (WHEEL_DIAMETER*Math.PI); - } - - private double inchesToNative(double inches){ - double rotations = inches / (WHEEL_DIAMETER*Math.PI); - return rotations * (tcd.leftMaster.encoderCPR*4); - } - - private String pointToString(CANTalon.TrajectoryPoint point){ - return "(pos = " + nativeToInches(point.position) + " in, vel = " + tcd.leftMaster.nativeToRPS(point.velocity) + " rps, dT = " + point.timeDurMs+" ms)"; + Robot.commandFinished = true; + System.out.println("ExecuteProfile interrupted!"); } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java new file mode 100644 index 00000000..1c5ca6c2 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java @@ -0,0 +1,26 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster.commands; + +import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.usfirst.frc.team449.robot.ReferencingCommand; +import org.usfirst.frc.team449.robot.ReferencingCommandGroup; +import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; + +/** + * Created by Noah Gleason on 4/6/2017. + */ +public class JiggleRobot extends ReferencingCommandGroup{ + /** + * Instantiate the ReferencingCommandGroup + * + * @param mappedSubsystem the {@link MappedSubsystem} + * to feed to this + * {@code + * ReferencingCommandGroup}'s + * {@link ReferencingCommand}s + */ + public JiggleRobot(TalonClusterDrive mappedSubsystem) { + super(mappedSubsystem); + addSequential(new NavXRelativeTTA(mappedSubsystem.turnPID, 10, mappedSubsystem, 3)); + addSequential(new NavXRelativeTTA(mappedSubsystem.turnPID, -10, mappedSubsystem, 3)); + } +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/LogError.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/LogError.java new file mode 100644 index 00000000..30f4754f --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/LogError.java @@ -0,0 +1,63 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster.commands; + +import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.usfirst.frc.team449.robot.ReferencingCommand; +import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; + +/** + * A wrapper command that switches to low gear. + */ +public class LogError extends ReferencingCommand { + + private String error; + + /** + * Default constructor + * @param subsystem The subsystem to execute this command on + */ + public LogError(MappedSubsystem subsystem, String error) { + super(subsystem); + this.error = error; + } + + /** + * Log when this command is initialized + */ + @Override + protected void initialize() { + System.out.println("LogError init."); + } + + /** + * Switch to low gear + */ + @Override + protected void execute() { + ((TalonClusterDrive) subsystem).logError(error); + } + + /** + * Finish immediately because this is a state-change command. + * @return true + */ + @Override + protected boolean isFinished() { + return true; + } + + /** + * Log when this command ends + */ + @Override + protected void end() { + System.out.println("LogError end."); + } + + /** + * Log when this command is interrupted. + */ + @Override + protected void interrupted() { + System.out.println("LogError Interrupted!"); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/PIDTest.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/PIDTest.java index 07ceaea9..b57b0881 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/PIDTest.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/PIDTest.java @@ -14,15 +14,15 @@ public class PIDTest extends ReferencingCommandGroup { * Default constructor * @param mappedSubsystem the TalonClusterDrive to execute this command on */ - public PIDTest(MappedSubsystem mappedSubsystem) { + public PIDTest(MappedSubsystem mappedSubsystem, double driveTime) { super(mappedSubsystem); requires(mappedSubsystem); TalonClusterDrive driveSubsystem = (TalonClusterDrive) mappedSubsystem; //Drive forward for a bit - addSequential(new DriveAtSpeed(driveSubsystem, 0.5, 2.5)); + addSequential(new DriveAtSpeed(driveSubsystem, 0.7, driveTime)); //Stop - addSequential(new DriveAtSpeed(driveSubsystem, 0, 10)); + addSequential(new DriveAtSpeed(driveSubsystem, 0, 100)); } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MPLoader.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MPLoader.java new file mode 100644 index 00000000..34ca1b80 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MPLoader.java @@ -0,0 +1,66 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster.util; + +import com.ctre.CANTalon; +import edu.wpi.first.wpilibj.Notifier; +import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; + +import java.util.List; + +/** + * Created by BlairRobot on 2017-03-15. + */ +public class MPLoader { + + public static void loadTopLevel(MotionProfileData data, RotPerSecCANTalonSRX talon, double wheelDiameter){ + // Fill the Talon's buffer with points + talon.canTalon.disable(); + talon.canTalon.clearMotionProfileHasUnderrun(); + talon.canTalon.clearMotionProfileTrajectories(); + CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint(); + for (int i = 0; i < data.data.length; ++i) { + // Set all the fields of the profile point + point.position = feetToNative(data.data[i][0], talon.encoderCPR, wheelDiameter); + point.velocity = feetPerSecToNative(data.data[i][1], talon, wheelDiameter); + point.timeDurMs = (int) (data.data[i][2] * 1000.); + point.profileSlotSelect = 1; // gain selection + point.velocityOnly = false; // true => no position servo just velocity feedforward + point.zeroPos = i == 0; // If its the first point, zeroPos = true + point.isLastPoint = (i + 1) == data.data.length; // If its the last point, isLastPoint = true + + // Send the point to the Talon's buffer + if(!talon.canTalon.pushMotionProfileTrajectory(point)) { + System.out.println("Buffer full!"); + break; + } + } + } + + public static Notifier startLoadBottomLevel(List talons, double updateRate){ + MPUpdaterProcess updater = new MPUpdaterProcess(); + for (RotPerSecCANTalonSRX talon : talons){ + updater.addTalon(talon.canTalon); + } + Notifier updaterNotifier = new Notifier(updater); + updaterNotifier.startPeriodic(updateRate); + System.out.println("Started the notifier for "+talons.size()+" talons."); + return updaterNotifier; + } + + public static double nativeToFeet(double nativeUnits, int encoderCPR, double wheelDiameter){ + double rotations = nativeUnits / (encoderCPR*4); + return rotations * (wheelDiameter*Math.PI); + } + + public static double feetToNative(double feet, int encoderCPR, double wheelDiameter){ + double rotations = feet / (wheelDiameter*Math.PI); + return rotations * (encoderCPR*4); + } + + public static double feetPerSecToNative(double feet, RotPerSecCANTalonSRX talon, double wheelDiameter){ + return talon.RPStoNative(feet/(wheelDiameter*Math.PI)); + } + + public static double nativeToFeetPerSec(double nativeUnits, RotPerSecCANTalonSRX talon, double wheelDiameter){ + return talon.nativeToRPS(nativeUnits)*(wheelDiameter*Math.PI); + } +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MotionProfileData.java b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MotionProfileData.java index 9d4b17e2..7b4ff01f 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MotionProfileData.java +++ b/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/util/MotionProfileData.java @@ -16,10 +16,17 @@ public class MotionProfileData { private int dPtr = -1; - public MotionProfileData(String filename) { + private boolean inverted; + + public MotionProfileData(String filename, boolean inverted){ + this.inverted = inverted; readFile(filename); } + public MotionProfileData(String filename) { + this(filename, false); + } + private void readFile(String filename) { try (Stream stream = Files.lines(Paths.get(filename))) { stream.forEach(this::processLine); @@ -38,7 +45,12 @@ private void processLine(String line) { // Strip the end of line comma tokens[2] = tokens[2].replace(",",""); - data[dPtr] = Arrays.stream(tokens).mapToDouble(Double::parseDouble).toArray(); + double[] tmp = Arrays.stream(tokens).mapToDouble(Double::parseDouble).toArray(); + if (inverted) { + tmp[0] = -tmp[0]; + tmp[1] = -tmp[1]; + } + data[dPtr] = tmp; } dPtr++; } diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java new file mode 100644 index 00000000..55e96720 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java @@ -0,0 +1,52 @@ +package org.usfirst.frc.team449.robot.mechanism.activegear; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.VictorSP; +import org.usfirst.frc.team449.robot.MappedSubsystem; + +/** + * The subsystem that carries and pushes gears. + */ +public class ActiveGearSubsystem extends MappedSubsystem { + /** + * Piston for pushing gears + */ + private DoubleSolenoid piston; + /** + * Whether piston is currently contracted + */ + public boolean contracted; + + /** + * Creates a mapped subsystem and sets its map + * + * @param map the map of constants relevant to this subsystem + */ + public ActiveGearSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearMap.ActiveGear map) { + super(map.getMechanism()); + this.map = map; + this.piston = new DoubleSolenoid(map.getModuleNumber(), map.getPiston().getForward(), map.getPiston().getReverse()); + } + + /** + * Fire the piston + * + * @param value direction to fire + */ + public void setPiston(DoubleSolenoid.Value value) { + piston.set(value); + System.out.println("Set Piston"); + contracted = (value == DoubleSolenoid.Value.kReverse); + } + + /** + * Initialize the default command for a subsystem. By default subsystems have + * no default command, but if they do, the default command is set with this + * method. It is called on all Subsystems by CommandBase in the users program + * after all the Subsystems are created. + */ + @Override + protected void initDefaultCommand() { + //Do nothing! + } +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/commands/FirePiston.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/commands/FirePiston.java new file mode 100644 index 00000000..478380e8 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/commands/FirePiston.java @@ -0,0 +1,57 @@ +package org.usfirst.frc.team449.robot.mechanism.activegear.commands; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import org.usfirst.frc.team449.robot.ReferencingCommand; +import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; + +/** + * Command for raising the intake + */ +public class FirePiston extends ReferencingCommand { + /** + * The active gear subsystem to execute this command on + */ + private ActiveGearSubsystem activeGearSubsystem; + + private DoubleSolenoid.Value position; + + /** + * Construct a FirePiston command + * + * @param activeGearSubsystem active gear subsystem to execute this command on + * @param position The direction to set the piston to be in + */ + public FirePiston(ActiveGearSubsystem activeGearSubsystem, DoubleSolenoid.Value position) { + super(activeGearSubsystem); + this.activeGearSubsystem = activeGearSubsystem; + this.position = position; + System.out.println("FirePiston constructed"); + } + + @Override + protected void initialize(){ + System.out.println("FirePiston init"); + } + + /** + * Set the piston to be in up position + */ + @Override + protected void execute() { + activeGearSubsystem.setPiston(position); + } + + /** + * Finish immediately because this is a state-change command. + * @return true + */ + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end(){ + System.out.println("FirePiston end"); + } +} diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java index edf712e3..0dc404d0 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java @@ -1,10 +1,11 @@ package org.usfirst.frc.team449.robot.mechanism.climber; +import edu.wpi.first.wpilibj.VictorSP; import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; import org.usfirst.frc.team449.robot.mechanism.MechanismSubsystem; /** - * The climber, with current monitoring to stop. + * The climber, with power monitoring to stop. */ public class ClimberSubsystem extends MechanismSubsystem { /** @@ -13,9 +14,10 @@ public class ClimberSubsystem extends MechanismSubsystem { public RotPerSecCANTalonSRX canTalonSRX; /** - * The maximum allowable current before we stop the motor. + * The maximum allowable power before we stop the motor. */ - private double max_current; + private double max_power; + private VictorSP victor; /** * Construct a ClimberSubsystem @@ -26,7 +28,11 @@ public ClimberSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.climber.Cli //Instantiate things this.map = map; canTalonSRX = new RotPerSecCANTalonSRX(map.getWinch()); - this.max_current = map.getMaxCurrent(); + this.max_power = map.getMaxPower(); + if (map.hasVictor()) { + this.victor = new VictorSP(map.getVictor().getPort()); + victor.setInverted(map.getVictor().getInverted()); + } } /** @@ -46,13 +52,16 @@ protected void initDefaultCommand() { */ public void setPercentVbus(double percentVbus) { canTalonSRX.setPercentVbus(percentVbus); + if (victor != null) { + victor.set(percentVbus); + } } /** - * Whether or not the current limit has been reached. - * @return If the output current is greater than the max allowable current. + * Whether or not the power limit has been reached. + * @return If the output power is greater than the max allowable power. */ public boolean reachedTop() { - return canTalonSRX.canTalon.getOutputCurrent() > max_current; + return Math.abs(canTalonSRX.getPower()) > max_power; } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/ManualClimb.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/ManualClimb.java index f719808f..6dd8da97 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/ManualClimb.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/ManualClimb.java @@ -21,7 +21,7 @@ public ManualClimb(ClimberSubsystem climber) { super(climber); requires(climber); this.climber = climber; - System.out.println("CurrentClimb constructed"); + System.out.println("PowerClimb constructed"); } /** @@ -29,7 +29,7 @@ public ManualClimb(ClimberSubsystem climber) { */ @Override protected void initialize() { - System.out.println("CurrentClimb init"); + System.out.println("PowerClimb init"); } /** diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/CurrentClimb.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/PowerClimb.java similarity index 59% rename from src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/CurrentClimb.java rename to src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/PowerClimb.java index eee42e1f..8b28586b 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/CurrentClimb.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/PowerClimb.java @@ -5,24 +5,28 @@ import org.usfirst.frc.team449.robot.mechanism.climber.ClimberSubsystem; /** - * Climb the rope and stop when the current limit is exceeded. + * Climb the rope and stop when the power limit is exceeded. */ -public class CurrentClimb extends ReferencingCommand { +public class PowerClimb extends ReferencingCommand { /** * The climber to execute this command on */ private ClimberSubsystem climber; + private long startTime; + + private long spinupTime = 250; + /** * Default constructor * @param climber The climber subsystem to execute this command on */ - public CurrentClimb(ClimberSubsystem climber) { + public PowerClimb(ClimberSubsystem climber) { super(climber); requires(climber); this.climber = climber; - System.out.println("CurrentClimb constructed"); + System.out.println("PowerClimb constructed"); } /** @@ -30,27 +34,28 @@ public CurrentClimb(ClimberSubsystem climber) { */ @Override protected void initialize() { - System.out.println("CurrentClimb init"); + System.out.println("PowerClimb init"); + startTime = System.currentTimeMillis(); } /** - * Climb at full speed and log the current + * Climb at full speed and log the power */ @Override protected void execute() { //Climb as fast as we can climber.setPercentVbus(1); - //Log current to SmartDashboard - SmartDashboard.putNumber("Current", climber.canTalonSRX.canTalon.getOutputCurrent()); + //Log power to SmartDashboard + SmartDashboard.putNumber("Power", climber.canTalonSRX.getPower()); } /** - * Stop when the current limit is exceeded. - * @return true when the current limit is exceed, false otherwise. + * Stop when the power limit is exceeded. + * @return true when the power limit is exceed, false otherwise. */ @Override protected boolean isFinished() { - return climber.reachedTop(); + return climber.reachedTop() && System.currentTimeMillis() - startTime > spinupTime; } /** @@ -60,7 +65,7 @@ protected boolean isFinished() { protected void end() { //Stop the motor when we reach the top. climber.setPercentVbus(0); - System.out.println("CurrentClimb end"); + System.out.println("PowerClimb end"); } /** @@ -70,7 +75,7 @@ protected void end() { protected void interrupted() { //Stop climbing if we're for some reason interrupted. climber.setPercentVbus(0); - System.out.println("CurrentClimb interrupted, stopping climb."); + System.out.println("PowerClimb interrupted, stopping climb."); } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/StopClimbing.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/StopClimbing.java index b9b0fd7b..4e05b1a7 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/StopClimbing.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/StopClimbing.java @@ -21,7 +21,7 @@ public StopClimbing(ClimberSubsystem climber) { super(climber); requires(climber); this.climber = climber; - System.out.println("CurrentClimb constructed"); + System.out.println("PowerClimb constructed"); } /** @@ -29,7 +29,7 @@ public StopClimbing(ClimberSubsystem climber) { */ @Override protected void initialize() { - System.out.println("CurrentClimb init"); + System.out.println("PowerClimb init"); } /** diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java index 8a778eca..60af5391 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java @@ -41,9 +41,13 @@ public Intake2017(maps.org.usfirst.frc.team449.robot.mechanism.intake.Intake2017 this.map = map; this.fixedVictor = new VictorSP(map.getFixedVictor().getPort()); fixedVictor.setInverted(map.getFixedVictor().getInverted()); - this.actuatedVictor = new VictorSP(map.getActuatedVictor().getPort()); - actuatedVictor.setInverted(map.getActuatedVictor().getInverted()); - this.piston = new DoubleSolenoid(map.getPistonModuleNum(), map.getPiston().getForward(), map.getPiston().getReverse()); + if (map.hasActuatedVictor()) { + this.actuatedVictor = new VictorSP(map.getActuatedVictor().getPort()); + actuatedVictor.setInverted(map.getActuatedVictor().getInverted()); + } + if (map.hasPiston()) { + this.piston = new DoubleSolenoid(map.getPistonModuleNum(), map.getPiston().getForward(), map.getPiston().getReverse()); + } } /** @@ -61,7 +65,9 @@ public void setFixedVictor(double speed) { * @param speed PWM setpoint [-1, 1] */ public void setActuatedVictor(double speed) { - actuatedVictor.set(speed); + if (actuatedVictor != null) { + actuatedVictor.set(speed); + } } /** @@ -70,9 +76,11 @@ public void setActuatedVictor(double speed) { * @param value direction to fire */ public void setPiston(DoubleSolenoid.Value value) { - piston.set(value); - System.out.println("Set Piston"); - intakeUp = (value == DoubleSolenoid.Value.kReverse); + if (piston != null) { + piston.set(value); + System.out.println("Set Piston"); + intakeUp = (value == DoubleSolenoid.Value.kReverse); + } } /** diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java index ac3b3550..d5cba2bf 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java @@ -15,7 +15,7 @@ public class SingleFlywheelShooter extends MappedSubsystem { /** * The flywheel's Talon */ - private RotPerSecCANTalonSRX talon; + public RotPerSecCANTalonSRX talon; /** * Whether the flywheel is currently commanded to spin diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/AccelerateFlywheel.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/AccelerateFlywheel.java index 55eaf255..98b9d1c9 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/AccelerateFlywheel.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/AccelerateFlywheel.java @@ -39,13 +39,13 @@ protected void initialize() { */ @Override protected void execute() { + flywheelShooter.talon.canTalon.enable(); flywheelShooter.logData(((SingleFlywheelShooter) subsystem).throttle); flywheelShooter.setDefaultSpeed(((SingleFlywheelShooter) subsystem).throttle); flywheelShooter.spinning = true; System.out.println("AccelerateFlywheel executed"); } - //TODO make AccelerateFlywheel and DecelerateFlywheel consistent here. /** * Finish immediately because this is a state-change command. * @return true diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/DecelerateFlywheel.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/DecelerateFlywheel.java index 8a6a703e..79663484 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/DecelerateFlywheel.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/commands/DecelerateFlywheel.java @@ -42,18 +42,17 @@ protected void execute() { flywheelShooter.logData(0.0); flywheelShooter.setDefaultSpeed(0.0); flywheelShooter.spinning = false; + flywheelShooter.talon.canTalon.disable(); System.out.println("DecelerateFlywheel executed"); } - //TODO make AccelerateFlywheel and DecelerateFlywheel consistent here. /** - * Runs constantly in order to log data. - * - * @return false + * Finish immediately because this is a state-change command. + * @return true */ @Override protected boolean isFinished() { - return false; + return true; } /** diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java index 371f9138..170566ea 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java @@ -21,9 +21,14 @@ public class FireShooter extends CommandGroup { * @param feeder feeder subsystem */ public FireShooter(SingleFlywheelShooter sfs, Intake2017 intake, FeederSubsystem feeder) { - requires(intake); - addParallel(new AccelerateFlywheel(sfs, 2.5 * 60)); - addParallel(new StaticInDynamicStop(intake)); - addParallel(new RunFeeder(feeder)); + if (sfs != null) { + addParallel(new AccelerateFlywheel(sfs, 2.5 * 60)); + } + if (intake != null) { + addParallel(new StaticInDynamicStop(intake)); + } + if (feeder != null) { + addParallel(new RunFeeder(feeder)); + } } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java index 87164cf4..35d7076f 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java @@ -22,10 +22,15 @@ public class LoadShooter extends CommandGroup { * @param feeder feeder subsystem */ public LoadShooter(SingleFlywheelShooter sfs, Intake2017 intake, FeederSubsystem feeder) { - requires(intake); - addParallel(new DecelerateFlywheel(sfs, 5)); - addParallel(new IntakeDown(intake)); - addParallel(new StaticInDynamicIn(intake)); - addParallel(new StopFeeder(feeder)); + if (sfs != null) { + addParallel(new DecelerateFlywheel(sfs, 5)); + } + if (intake != null) { + addParallel(new IntakeDown(intake)); + addParallel(new StaticInDynamicIn(intake)); + } + if (feeder != null) { + addParallel(new StopFeeder(feeder)); + } } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java index fea0d865..d5241121 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java @@ -22,10 +22,15 @@ public class RackShooter extends CommandGroup { * @param feeder feeder subsystem */ public RackShooter(SingleFlywheelShooter sfs, Intake2017 intake, FeederSubsystem feeder) { - requires(intake); - addParallel(new AccelerateFlywheel(sfs, 5)); - addParallel(new IntakeUp(intake)); - addParallel(new StaticInDynamicStop(intake)); - addParallel(new StopFeeder(feeder)); + if (sfs != null) { + addParallel(new AccelerateFlywheel(sfs, 5)); + } + if (intake != null) { + addParallel(new IntakeUp(intake)); + addParallel(new StaticInDynamicStop(intake)); + } + if (feeder != null) { + addParallel(new StopFeeder(feeder)); + } } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java index c0c2c5e0..2e1ebfdf 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java +++ b/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java @@ -22,11 +22,16 @@ public class ResetShooter extends CommandGroup { * @param feeder feeder subsystem */ public ResetShooter(SingleFlywheelShooter sfs, Intake2017 intake, FeederSubsystem feeder) { - requires(intake); - addParallel(new DecelerateFlywheel(sfs, 5)); + if (sfs != null) { + addParallel(new DecelerateFlywheel(sfs, 5)); + } //TODO Possibly have this raise intake instead because we'll mostly use this right before climbing. - addParallel(new IntakeDown(intake)); - addParallel(new StaticStopDynamicStop(intake)); - addParallel(new StopFeeder(feeder)); + if (intake != null) { + addParallel(new IntakeDown(intake)); + addParallel(new StaticStopDynamicStop(intake)); + } + if (feeder != null) { + addParallel(new StopFeeder(feeder)); + } } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/oi/MappedJoystickButton.java b/src/main/java/org/usfirst/frc/team449/robot/oi/MappedJoystickButton.java index a1c98818..698b4bd2 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/oi/MappedJoystickButton.java +++ b/src/main/java/org/usfirst/frc/team449/robot/oi/MappedJoystickButton.java @@ -1,8 +1,11 @@ package org.usfirst.frc.team449.robot.oi; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import maps.org.usfirst.frc.team449.robot.oi.JoystickButtonMap; +import org.usfirst.frc.team449.robot.components.TriggerButton; +import org.usfirst.frc.team449.robot.components.dPadButton; /** * A button with a port and joystick that can be constructed from a map object. @@ -14,6 +17,16 @@ public class MappedJoystickButton extends JoystickButton { * @param map config map */ public MappedJoystickButton(JoystickButtonMap.JoystickButton map) { - super(new Joystick(map.getJoystickIndex()), map.getButtonIndex()); + super(new Joystick(map.getPort()), map.getButtonIndex()); + } + + public static Button constructButton(JoystickButtonMap.JoystickButton map){ + if (map.hasTriggerAt()){ + return new TriggerButton(map); + } else if(map.hasAngle()) { + return new dPadButton(map); + } else { + return new MappedJoystickButton(map); + } } } diff --git a/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017.java b/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017.java index c5235b05..1d8880cd 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017.java +++ b/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017.java @@ -73,7 +73,7 @@ public void mapButtons() { tt180.whenPressed(new NavXTurnToAngle(Robot.driveSubsystem.turnPID, 180, Robot.driveSubsystem, 2.5)); tt330.whenPressed(new NavXTurnToAngle(Robot.driveSubsystem.turnPID, -30, Robot.driveSubsystem, 2.5)); driveStraight.whileHeld(new NavXDriveStraight(Robot.driveSubsystem.straightPID, Robot.driveSubsystem, this)); - // climbButton.whileHeld(new CurrentClimb(Robot.climberSubsystem)); + // climbButton.whileHeld(new PowerClimb(Robot.climberSubsystem)); // toggleFlywheel.whenPressed(new org.usfirst.frc.team449.robot.mechanism.doubleflywheelshooter.commands // .ToggleFlywheel(Robot.doubleFlywheelShooterSubsystem)); // toggleFlywheel.whenPressed(new org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.commands diff --git a/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java b/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java index 032e37d5..17e03c26 100644 --- a/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java +++ b/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java @@ -1,26 +1,29 @@ package org.usfirst.frc.team449.robot.oi; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; -import edu.wpi.first.wpilibj.buttons.JoystickButton; +import maps.org.usfirst.frc.team449.robot.oi.JoystickButtonMap; import maps.org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepadMap; import org.usfirst.frc.team449.robot.Robot; -import org.usfirst.frc.team449.robot.components.TriggerButton; import org.usfirst.frc.team449.robot.drive.talonCluster.commands.*; import org.usfirst.frc.team449.robot.drive.talonCluster.commands.ois.ArcadeOI; -import org.usfirst.frc.team449.robot.mechanism.climber.commands.CurrentClimb; +import org.usfirst.frc.team449.robot.mechanism.activegear.commands.FirePiston; +import org.usfirst.frc.team449.robot.mechanism.climber.commands.ManualClimb; +import org.usfirst.frc.team449.robot.mechanism.climber.commands.PowerClimb; import org.usfirst.frc.team449.robot.mechanism.climber.commands.StopClimbing; import org.usfirst.frc.team449.robot.mechanism.feeder.commands.ToggleFeeder; import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.commands.ToggleIntakeUpDown; import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.commands.ToggleIntaking; import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.commands.ToggleShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.FireShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.LoadShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.RackShooter; +import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.*; import org.usfirst.frc.team449.robot.oi.components.SmoothedThrottle; import org.usfirst.frc.team449.robot.oi.components.Throttle; import org.usfirst.frc.team449.robot.vision.commands.ChangeCam; +import java.util.ArrayList; +import java.util.List; + /** * An OI for using an Xbox-style controller for an arcade drive, where one stick controls forward velocity and the other * controls turning velocity. @@ -135,6 +138,16 @@ public class OI2017ArcadeGamepad extends BaseOI implements ArcadeOI { */ private Button fireShooter; + private Button resetShooter; + + private Button toggleGear; + private List