diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt index da2cacf..a613e59 100644 --- a/AAAScripts/scripts.txt +++ b/AAAScripts/scripts.txt @@ -9,7 +9,6 @@ moveto (0,120) #Drive past auto line RRxx: moveto (0,155) turn -90 -move 10 switch RLxx: @@ -17,7 +16,7 @@ move 56 turn -90 move 170 turn 90 -move 35 +move 25 switch @@ -35,7 +34,6 @@ exchange RRxE: moveto (0,155) turn -90 -move 12 switch turn -90 move 105 @@ -55,7 +53,7 @@ move 56 turn -90 move 170 turn 90 -move 35 +move 25 switch move -40 turn 90 @@ -80,7 +78,6 @@ moveto (0, 120) #Drive past auto line LLxx: move 150 turn 90 -move 12 switch LRxx: @@ -88,7 +85,7 @@ move 56 turn 90 move 170 turn -90 -move 35 +move 25 switch @@ -106,7 +103,6 @@ exchange LLxE: move 150 turn 90 -move 12 switch turn 90 move 95 @@ -126,7 +122,7 @@ move 56 turn 90 move 170 turn -90 -move 35 +move 25 switch move -40 turn -90 @@ -148,17 +144,17 @@ moveto (0,50) (48,50) (48,92) #cross baseline # Switch CRxx: -moveto (0,50) (48,50) (48,95) +moveto (0,50) (48,50) (48,85) switch #deploy switch CLxx: -moveto (0,50) (-56,50) (-56,95) +moveto (0,50) (-56,50) (-56,85) switch #deploy switch # Switch and Exchange CRxE: -moveto (0,50) (48,50) (48,95) +moveto (0,50) (48,50) (48,85) switch #deploy switch move -48 # 3 feet back turn -90 @@ -173,7 +169,7 @@ exchange CLxE: -moveto (0,50) (-56,50) (-56,95) +moveto (0,50) (-56,50) (-56,85) switch #deploy switch move -48 # 3 feet back turn 90 diff --git a/CompSD.xml b/CompSD.xml new file mode 100644 index 0000000..30dfbca --- /dev/null +++ b/CompSD.xml @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 93 + 78 + + + + 586 + 430 + + + + + 630 + 459 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index e8d42ba..c4d2111 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -7,22 +7,23 @@ package org.usfirst.frc.team199.Robot2018; -import org.usfirst.frc.team199.Robot2018.commands.CloseIntake; +import org.usfirst.frc.team199.Robot2018.commands.AutoLift; import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant; import org.usfirst.frc.team199.Robot2018.commands.IntakeCube; -import org.usfirst.frc.team199.Robot2018.commands.OpenIntake; +import org.usfirst.frc.team199.Robot2018.commands.MoveLift; +import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID; import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube; import org.usfirst.frc.team199.Robot2018.commands.PIDMove; import org.usfirst.frc.team199.Robot2018.commands.PIDTurn; import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders; -import org.usfirst.frc.team199.Robot2018.commands.RunLift; import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse; import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType; import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear; import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear; -import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake; -import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake; +import org.usfirst.frc.team199.Robot2018.commands.StopIntake; +import org.usfirst.frc.team199.Robot2018.commands.ToggleIntake; import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -48,6 +49,9 @@ public class OI { private JoystickButton resetEncButton; private JoystickButton moveLiftUpButton; private JoystickButton moveLiftDownButton; + private JoystickButton moveLiftPIDUpButton; + private JoystickButton moveLiftPIDDownButton; + private JoystickButton testLiftPID; private JoystickButton findTurnTimeConstantButton; private JoystickButton updatePIDConstantsButton; private JoystickButton updateEncoderDPPButton; @@ -61,6 +65,8 @@ public class OI { private JoystickButton outakeCubeButton; private JoystickButton toggleLeftIntakeButton; private JoystickButton toggleRightIntakeButton; + private JoystickButton toggleIntakeButton; + private JoystickButton stopIntakeButton; public int getButton(String key, int def) { if (!SmartDashboard.containsKey("Button/" + key)) { @@ -92,6 +98,10 @@ public OI(Robot robot) { findTurnTimeConstantButton .whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd)); + testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5)); + testLiftPID.whenPressed( + new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString()))); + rightJoy = new Joystick(1); shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4)); shiftHighGearButton.whenPressed(new ShiftHighGear()); @@ -104,34 +114,46 @@ public OI(Robot robot) { updateEncoderDPPButton.whenPressed(new SetDistancePerPulse()); moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10)); - moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); + moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true)); moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11)); - moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); + moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false)); manipulator = new Joystick(2); - if (manipulator.getButtonCount() == 0) { - System.err.println( - "ERROR: manipulator does not appear to be plugged in. Disabling intake code. Restart code with manipulator plugged in to enable intake code"); - } else { - closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1)); - closeIntakeButton.whenPressed(new CloseIntake()); - openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2)); - openIntakeButton.whenPressed(new OpenIntake()); - // raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake - // Button", 3)); - // raiseIntake.whenPressed(new RaiseIntake()); - // lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake - // Button", 4)); - // lowerIntake.whenPressed(new LowerIntake()); - intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5)); - intakeCubeButton.whenPressed(new IntakeCube()); - outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6)); - outakeCubeButton.whenPressed(new OuttakeCube()); - toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3)); - toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake()); - toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4)); - toggleRightIntakeButton.whenPressed(new ToggleRightIntake()); - } + // if (manipulator.getButtonCount() == 0) { + // System.err.println( + // "ERROR: manipulator does not appear to be plugged in. Disabling intake code. + // Restart code with manipulator plugged in to enable intake code"); + // } else { + + // closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake + // Button", 1)); + // closeIntakeButton.whenPressed(new CloseIntake()); + // openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake + // Button", 2)); + // openIntakeButton.whenPressed(new OpenIntake()); + + moveLiftPIDUpButton = new JoystickButton(manipulator, getButton("Run Lift PID Up", 4)); + moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true)); + moveLiftPIDDownButton = new JoystickButton(manipulator, getButton("Run Lift PID Down", 1)); + moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false)); + + toggleIntakeButton = new JoystickButton(manipulator, getButton("Toggle Intake Button", 2)); + toggleIntakeButton.whenPressed(new ToggleIntake()); + + stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3)); + stopIntakeButton.whenPressed(new StopIntake()); + + intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5)); + intakeCubeButton.whenPressed(new IntakeCube()); + outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6)); + outakeCubeButton.whenPressed(new OuttakeCube()); + // toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle + // Left Intake Button", 3)); + // toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake()); + // toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle + // Right Intake Button", 4)); + // toggleRightIntakeButton.whenPressed(new ToggleRightIntake()); + // } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 045fda7..b85b57d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -23,6 +23,7 @@ import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject; import org.usfirst.frc.team199.Robot2018.subsystems.Lift; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.PIDController; @@ -52,6 +53,8 @@ public class Robot extends IterativeRobot { public static OI oi; public static Map> autoScripts; + public static boolean auto; + public static boolean stopIntake = false; Command autonomousCommand; SendableChooser posChooser = new SendableChooser(); @@ -59,6 +62,7 @@ public class Robot extends IterativeRobot { String[] fmsPossibilities = { "LL", "LR", "RL", "RR" }; public static SmartDashboardInterface sd = new SmartDashboardInterface() { + @Override public double getConst(String key, double def) { Preferences pref = Preferences.getInstance(); if (!pref.containsKey("Const/" + key)) { @@ -71,6 +75,20 @@ public double getConst(String key, double def) { return pref.getDouble("Const/" + key, def); } + @Override + public String getString(String key, String def) { + Preferences pref = Preferences.getInstance(); + if (!pref.containsKey("String/" + key)) { + pref.putString("String/" + key, def); + if (pref.getString("String/ + key", def) != def) { + System.err.println("pref Key" + "String/" + key + "already taken by a different type"); + return def; + } + } + return pref.getString("String/" + key, def); + } + + @Override public void putConst(String key, double def) { Preferences pref = Preferences.getInstance(); pref.putDouble("Const/" + key, def); @@ -79,14 +97,17 @@ public void putConst(String key, double def) { } } + @Override public void putData(String string, PIDController controller) { SmartDashboard.putData(string, controller); } + @Override public void putNumber(String string, double d) { SmartDashboard.putNumber(string, d); } + @Override public void putBoolean(String string, boolean b) { SmartDashboard.putBoolean(string, b); } @@ -104,6 +125,10 @@ public static double getConst(String key, double def) { return sd.getConst(key, def); } + public static String getString(String key, String def) { + return sd.getString(key, def); + } + public static void putConst(String key, double def) { sd.putConst(key, def); } @@ -157,8 +182,9 @@ public void robotInit() { autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", "")); listen = new Listener(); - // CameraServer.getInstance().startAutomaticCapture(0); - // CameraServer.getInstance().startAutomaticCapture(1); + lift.resetEnc(); + CameraServer.getInstance().startAutomaticCapture(0); + CameraServer.getInstance().startAutomaticCapture(1); } /** @@ -169,6 +195,7 @@ public void robotInit() { @Override public void disabledInit() { dt.disableVelocityPIDs(); + lift.setSetpoint(0); } @Override @@ -183,6 +210,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { + auto = true; dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); Scheduler.getInstance().add(new ShiftLowGear()); @@ -212,6 +240,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + auto = false; System.out.println("In teleopInit()"); dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); @@ -245,7 +274,8 @@ public void teleopPeriodic() { // SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist()); // - System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get()); + // System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), + // RobotMap.dtRight.get()); SmartDashboard.putNumber("Angle", dt.getAHRSAngle()); SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4)); @@ -259,6 +289,7 @@ public void testInit() { System.out.println("In testInit()"); dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); + lift.disable(); } /** @@ -266,43 +297,8 @@ public void testInit() { */ @Override public void testPeriodic() { - // if(firstTime) { - // Robot.dt.enableVelocityPIDs(); - // firstTime = false; - //// } - // dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0, - // getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())), - // /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF", - // 1 / Robot.getConst("Max Low Speed", 84))); - // dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0, - // getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())), - // /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF", - // 1 / Robot.getConst("Max Low Speed", 84))); - // dt.resetAllVelocityPIDConsts(); - - // dt.setVPIDs(getConst("VPID Test Set", 0.5)); - - Scheduler.getInstance().run(); - - // System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput()); - // System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput()); - // System.out.println("Left VPID Error: " + dt.getLeftVPIDerror()); - // System.out.println("Right VPID Error: " + dt.getRightVPIDerror()); - // System.out.println("Left Enc Rate: " + dt.getLeftEncRate()); - // System.out.println("Right Enc Rate: " + dt.getRightEncRate()); - // - // System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get()); - // System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get()); - - // System.out.println("Left Enc Dist " + dt.getLeftDist()); - // System.out.println("Right Enc Dist " + dt.getRightDist()); - // System.out.println("Avg Enc Dist" + dt.getEncAvgDist()); - - // dt.dtLeft.set(0.1); - // dt.dtRight.set(-oi.rightJoy.getY()); - // dt.dtLeft.set(-oi.leftJoy.getY()); - // dt.dtRight.set(-oi.rightJoy.getY()); - - // dt.putVelocityControllersToDashboard(); + // Scheduler.getInstance().run(); + // lift.disable(); + // lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0)); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index af730f8..bb50606 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -43,7 +44,7 @@ public class RobotMap { public static DigitalSource liftEncPort1; public static DigitalSource liftEncPort2; - //public static WPI_TalonSRX climberMotor; + // public static WPI_TalonSRX climberMotor; public static VictorSP leftIntakeMotor; public static VictorSP rightIntakeMotor; @@ -123,18 +124,25 @@ private void configSPX(WPI_VictorSPX mc) { public RobotMap() { pdp = new PowerDistributionPanel(); - + liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5)); configSPX(liftMotorA); liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6)); configSRX(liftMotorB); liftMotorC = new WPI_TalonSRX(getPort("2LiftTalonSRX", 7)); configSRX(liftMotorC); - liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC); + // liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC); + liftMotors = new SpeedControllerGroup(liftMotorC); + liftMotors.setName("Lift", "CIM Motor"); + liftMotors.setInverted(true); + LiveWindow.add(liftMotors); + liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4)); liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5)); liftEnc = new Encoder(liftEncPort1, liftEncPort2); liftEnc.setPIDSourceType(PIDSourceType.kDisplacement); + liftEnc.setName("Lift", "Encoder"); + LiveWindow.add(liftEnc); leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 8)); rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 9)); @@ -252,7 +260,10 @@ public double getRadius() { return Robot.getConst("Radius of Drivetrain Wheel", 0.0635); } - public double getOmegaMax() { + /** + * @return the RPM max of a CIM + */ + public static double getOmegaMax() { return Robot.getConst("Omega Max", 5330); } @@ -264,7 +275,10 @@ public double getCycleTime() { return Robot.getConst("Code cycle time", 0.05); } - public double getStallTorque() { + /** + * @return the stall torque of a CIM + */ + public static double getStallTorque() { return Robot.getConst("Stall Torque", 2.41); } @@ -273,7 +287,12 @@ private double convertLbsTokG(double lbs) { return lbs * 0.45359237; } - private double convertNtokG(double newtons) { + /** + * @param the + * weight (in Newtons) + * @return the equivalent mass (in kg) + */ + public static double convertNtokG(double newtons) { // weight / accel due to grav = kg return newtons / 9.81; } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java index 7dba315..fc36820 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java @@ -12,4 +12,6 @@ public interface SmartDashboardInterface { public void putNumber(String string, double d); public void putBoolean(String string, boolean b); + + public String getString(String key, String def); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py index 948a80f..54c95b1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py @@ -2,7 +2,7 @@ from networktables import NetworkTables import __future__ import time -NetworkTables.initialize(server='10.1.99.2') +NetworkTables.initialize(server='172.22.11.2') prefs = NetworkTables.getTable("Preferences") go = False #true when file has been successfully read diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java index 764551b..fb3fdbc 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java @@ -1,114 +1,21 @@ package org.usfirst.frc.team199.Robot2018.commands; -import org.usfirst.frc.team199.Robot2018.Robot; -import org.usfirst.frc.team199.Robot2018.RobotMap; -import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface; -import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position; +import org.usfirst.frc.team199.Robot2018.subsystems.Lift; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; /** * */ -public class AutoLift extends Command implements PIDOutput{ - /** - * All distances are measured from bottom of cube and + 3 inches for wiggle room for dropping cubes - * Also, actual distances are divided by 3 because according to cad, the lift will have a 1:3 ratio from winch - * to actual height. - */ - - /** - * Distance to switch - * 18.75 inches in starting position (this measurement is the fence that surrounds the switch) - * 21.75 / 3 for ratio = 7.25 - */ - private final double SWITCH_DIST = 7.25; - /** - * Distance to scale - * 5 feet starting - * 63 / 3 = 21 - */ - private final double SCALE_DIST = 21; - /** - * Distance to bar - * 87 / 3 = 29 - * 7 feet starting; bar distance should be changed because I'm not aware how climber mech will be positioned - */ - private final double BAR_DIST = 29; - private double desiredDist = 0; - private double currDist; - private LiftInterface lift; - private Position desiredPos; - - private PIDController liftController; - - public AutoLift(Position stage, LiftInterface lift) { - this.lift = lift; - requires(Robot.lift); - currDist = lift.getHeight(); - switch(stage) { - case GROUND: - desiredDist = -currDist; - break; - case SWITCH: - desiredDist = SWITCH_DIST - currDist; - break; - case SCALE: - desiredDist = SCALE_DIST - currDist; - break; - case BAR: - desiredDist = BAR_DIST - currDist; - break; - } - - liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0), - Robot.getConst("LiftkD", 0),RobotMap.liftEnc, this); - - desiredPos = stage; - } - - // Called just before this Command runs the first time - protected void initialize() { - // input is in inches - //liftController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204)); - // output in "motor units" (arcade and tank only accept values [-1, 1] - liftController.setOutputRange(-1.0, 1.0); - liftController.setContinuous(false); - //liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2)); - liftController.setSetpoint(desiredDist); - liftController.enable(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - if(liftController.onTarget()) { - lift.setCurrPosition(desiredPos); - return true; - } - return false; - } - - // Called once after isFinished returns true - protected void end() { - liftController.disable(); - liftController.free(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } - - @Override - public void pidWrite(double output) { - lift.runMotor(output); +public class AutoLift extends CommandGroup { + + public AutoLift(Lift lift, String height) { + if (height.equals("GROUND")) { + addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE"))); + addSequential(new LiftToPosition(lift, LiftHeight.toLH(height))); + } else { + addSequential(new LiftToPosition(lift, LiftHeight.toLH(height))); + } } - } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java index c7e7372..7ef26e7 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java @@ -18,36 +18,44 @@ public DefaultIntake() { } // Called just before this Command runs the first time + @Override protected void initialize() { - try { - Robot.oi.manipulator.getRawAxis(1); - } catch (NullPointerException e) { - System.err.println("[ERROR] Manipulator not plugged in."); - manipulatorPluggedIn = false; - } + // try { + // Robot.oi.manipulator.getRawAxis(1); + // } catch (NullPointerException e) { + // System.err.println("[ERROR] Manipulator not plugged in."); + // manipulatorPluggedIn = false; + // } } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { // 1 and 5 represent the axes' index in driver station - if (manipulatorPluggedIn) { - Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1)); - Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5)); - } + // if (manipulatorPluggedIn) { + // Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1)); + // Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5)); + // } + Robot.intakeEject.runLeftIntake(Robot.getConst("Intake Holding Speed", 0.15)); + Robot.intakeEject.runRightIntake(Robot.getConst("Intake Holding Speed", 0.15)); + } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { return false; } // Called once after isFinished returns true + @Override protected void end() { Robot.intakeEject.runIntake(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { end(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java index 9782f56..b4a3b38 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToExchange.java @@ -1,36 +1,16 @@ package org.usfirst.frc.team199.Robot2018.commands; -import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.CommandGroup; /** * */ -public class EjectToExchange extends Command { - - public EjectToExchange() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } +public class EjectToExchange extends CommandGroup { - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } + public EjectToExchange() { + addSequential(new AutoLift(Robot.lift, "GROUND")); + addSequential(new OuttakeCube()); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java index b8e8dfa..bc23e32 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java @@ -1,36 +1,21 @@ package org.usfirst.frc.team199.Robot2018.commands; -import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.CommandGroup; /** * */ -public class EjectToScale extends Command { - - public EjectToScale() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } +public class EjectToScale extends CommandGroup { + + public EjectToScale() { + addSequential(new AutoLift(Robot.lift, "SCALE")); + addSequential( + new PIDMove(Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg())); + addSequential(new OuttakeCube()); + addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd, + Robot.dt.getDistEncAvg())); + addSequential(new AutoLift(Robot.lift, "GROUND")); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java index a0df40b..11bf36a 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java @@ -1,36 +1,21 @@ package org.usfirst.frc.team199.Robot2018.commands; -import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.CommandGroup; /** * */ -public class EjectToSwitch extends Command { - - public EjectToSwitch() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } +public class EjectToSwitch extends CommandGroup { + + public EjectToSwitch() { + addSequential(new AutoLift(Robot.lift, "SWITCH")); + addSequential( + new PIDMove(Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg())); + addSequential(new OuttakeCube()); + addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd, + Robot.dt.getDistEncAvg())); + addSequential(new AutoLift(Robot.lift, "GROUND")); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java index eb8d6d9..2bc12e7 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/FindTurnTimeConstant.java @@ -20,20 +20,20 @@ public class FindTurnTimeConstant extends Command { private SmartDashboardInterface sd; private AHRS gyro; private Robot robot; - private Timer tim = new Timer(); - private ArrayList dataPoints = new ArrayList(); + private Timer tim; + private ArrayList dataPoints; private class DataPoint { public double seconds; - public double degreesPerSecond; + public double radiansPerSecond; - public DataPoint(double seconds, double degreesPerSecond) { + public DataPoint(double seconds, double radiansPerSecond) { this.seconds = seconds; - this.degreesPerSecond = degreesPerSecond; + this.radiansPerSecond = radiansPerSecond; } public String toString() { - return "Time: " + seconds + " seconds; rotational velocity: " + degreesPerSecond + " degrees per second"; + return "Time: " + seconds + " seconds; rotational velocity: " + radiansPerSecond + " radians per second"; } } @@ -53,12 +53,18 @@ public FindTurnTimeConstant(Robot robot, DrivetrainInterface dt, AHRS gyro, Smar // Called just before this Command runs the first time protected void initialize() { System.out.println("FindTurnTimeConstant is in init()"); - if (!robot.isTest()) { - System.out.println("but you're not in test mode."); - return; - } + // if (!robot.isTest()) { + // System.out.println("but you're not in test mode."); + // return; + // } + + // this should be done in low gear + Robot.dt.shiftGears(false); + + dataPoints = new ArrayList(); System.out.println("and you're in test mode"); + tim = new Timer(); tim.start(); // spin at full speed dt.arcadeDrive(0, 1); @@ -70,6 +76,7 @@ protected void execute() { DataPoint newMeasurement = new DataPoint(tim.get(), gyro.getRate()); System.out.println(newMeasurement); dataPoints.add(newMeasurement); + dt.arcadeDrive(0, 1); } // Make this return true when this Command no longer needs to run execute() @@ -78,11 +85,16 @@ protected boolean isFinished() { // true if the last and second-to-last rotational velocities measured are within // 5 degrees per second squared of each other. or if it's not a test, finish // immediately - if (numberOfDataPoints < 2) { + + // if we're still in the first 51 cycles (don't have enough data), then not + // finished. also, we're probably still accelerating if we haven't reached 3 + // radians per second (the max will be around 5). + if (numberOfDataPoints < 51 || Math.abs(dataPoints.get(numberOfDataPoints - 1).radiansPerSecond) < 3.0) { return false; } - return (Math.abs(dataPoints.get(numberOfDataPoints - 1).degreesPerSecond - - dataPoints.get(numberOfDataPoints - 2).degreesPerSecond) < 5) || !robot.isTest(); + + return (Math.abs(dataPoints.get(numberOfDataPoints - 1).radiansPerSecond + - dataPoints.get(numberOfDataPoints - 51).radiansPerSecond) < 0.02); } // Called once after isFinished returns true @@ -90,14 +102,17 @@ protected void end() { // stop moving dt.arcadeDrive(0, 0); - DataPoint lastDataPoint = dataPoints.get(dataPoints.size() - 1); - System.out.println("Finished when " + lastDataPoint); - // the rotvel of the data point we're looking for to find the time constant - double magicValue = lastDataPoint.degreesPerSecond * (1 - (1 / Math.E)); + double lastRadiansPerSecond = dataPoints.get(dataPoints.size() - 1).radiansPerSecond; + System.out.println("Finished at " + lastRadiansPerSecond + " radians per second"); + Robot.putConst("Max Turn Radians Per Second", lastRadiansPerSecond); + + // the radians per second of the data point we're looking for to find the time + // constant + double magicValue = lastRadiansPerSecond * (1 - (1 / Math.E)); for (DataPoint datum : dataPoints) { // if we've past the magic value, we're done - if (Math.abs(datum.degreesPerSecond) > Math.abs(magicValue)) { + if (Math.abs(datum.radiansPerSecond) > Math.abs(magicValue)) { System.out.println("The magic value is " + datum); Robot.putConst("TurnTimeConstant", datum.seconds); // stop checking data diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java index e95340e..041f4a2 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * @@ -16,22 +17,27 @@ public class IntakeCube extends Command { public IntakeCube() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + // requires(Robot.intakeEject); tim = new Timer(); } // Called just before this Command runs the first time + @Override protected void initialize() { tim.reset(); - tim.start(); + // tim.start(); overDraw = false; } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { Robot.intakeEject.runIntake(-1); + SmartDashboard.putBoolean("Has Cube", Robot.intakeEject.hasCube()); if (Robot.intakeEject.hasCube()) { if (!overDraw) { overDraw = true; + // tim.reset(); tim.start(); } } else { @@ -42,16 +48,21 @@ protected void execute() { } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { - return tim.get() > Robot.getConst("Has Cube Timeout", 0.5); + return tim.get() > Robot.getConst("Has Cube Timeout", 0.5) || Robot.stopIntake; } // Called once after isFinished returns true + @Override protected void end() { + Robot.intakeEject.stopIntake(); + Robot.stopIntake = false; } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java new file mode 100644 index 0000000..528a6d8 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java @@ -0,0 +1,57 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.subsystems.Lift; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class LiftToPosition extends Command { + + private Lift lift; + private LiftHeight pos; + private boolean goToGround; + + public LiftToPosition(Lift lift, LiftHeight goal) { + requires(Robot.lift); + this.lift = lift; + pos = goal; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0), + Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1)); + double setpoint = lift.getDesiredDistFromPos(pos); + lift.setSetpoint(setpoint); + System.out.println("Target Height: " + setpoint); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + System.out.println("Current Height: " + lift.getHeight()); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return lift.onTarget(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java new file mode 100644 index 0000000..6c25206 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java @@ -0,0 +1,36 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class MotionProfilingMove extends Command { + + public MotionProfilingMove() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java similarity index 61% rename from Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java rename to Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java index 1f5baae..9f2cb86 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java @@ -1,24 +1,22 @@ package org.usfirst.frc.team199.Robot2018.commands; import org.usfirst.frc.team199.Robot2018.Robot; -import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface; +import org.usfirst.frc.team199.Robot2018.subsystems.Lift; import edu.wpi.first.wpilibj.command.Command; /** - * + * Use this command to test the Lift without PID */ -public class RunLift extends Command { +public class MoveLift extends Command { - private LiftInterface lift; - private final double SPEED = 0.05; + private Lift lift; + private double SPEED; private int dir; - public RunLift(LiftInterface lift, boolean up) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - this.lift = lift; + public MoveLift(Lift lift, boolean up) { requires(Robot.lift); + this.lift = lift; if (up) dir = 1; else @@ -26,26 +24,37 @@ public RunLift(LiftInterface lift, boolean up) { } // Called just before this Command runs the first time + @Override protected void initialize() { + lift.disable(); + // below Lift Speed may be for manual testing only, but commented out code 2 + // lines below can be used if want max speed + SPEED = Robot.getConst("Lift Speed", 0.5); + // SPEED = lift.getLiftMaxSpeed(); } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { lift.runMotor(SPEED * dir); } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { return false; } // Called once after isFinished returns true + @Override protected void end() { lift.stopLift(); + lift.enable(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { end(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java new file mode 100644 index 0000000..888d4a8 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLiftWithPID.java @@ -0,0 +1,58 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.subsystems.Lift; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Use this command to test the Lift with PID + */ +public class MoveLiftWithPID extends Command { + + private Lift lift; + private double setpoint; + private int dir; + + public MoveLiftWithPID(Lift lift, boolean up) { + requires(Robot.lift); + this.lift = lift; + if (up) + dir = 1; + else + dir = -1; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // setpoint = lift.getPIDController().getSetpoint(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + setpoint = lift.getPIDController().getSetpoint() + dir * Robot.getConst("Lift Move Increment", 0.25); // inches/0.05 + // secs + lift.setSetpoint(setpoint); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + // always false bc whileHeld + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java index ad4ec0b..579c83d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java @@ -15,9 +15,11 @@ public class OuttakeCube extends Command { public OuttakeCube() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + requires(Robot.intakeEject); } // Called just before this Command runs the first time + @Override protected void initialize() { tim = new Timer(); tim.reset(); @@ -25,22 +27,26 @@ protected void initialize() { } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { Robot.intakeEject.runIntake(1); } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { return tim.get() > Robot.getConst("Outake Time", 0.5); } // Called once after isFinished returns true + @Override protected void end() { Robot.intakeEject.runIntake(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { end(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index ea24ee7..10cbd9e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -4,6 +4,7 @@ import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; @@ -23,6 +24,7 @@ public class PIDMove extends Command implements PIDOutput { private SmartDashboardInterface sd; private double[] point; private boolean absolute; + private boolean hasInitialized = false; /** * Constructs this command with a new PIDController. Sets all of the @@ -67,7 +69,7 @@ public PIDMove(double target, double[] point, DrivetrainInterface dt, SmartDashb double kP = r / Robot.rmap.getDrivetrainTimeConstant() / maxSpeed; double kI = 0; double kD = r / maxSpeed; - double kF = 1 / (maxSpeed * sd.getConst("Default PID Update Time", 0.05)) / maxSpeed; + double kF = 1 / maxSpeed * sd.getConst("Default PID Update Time", 0.05); moveController = new PIDController(kP, kI, kD, kF, avg, this); sd.putData("Move PID", moveController); @@ -120,6 +122,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s */ @Override public void initialize() { + hasInitialized = true; if (absolute) { double dx = point[0] - AutoUtils.state.getX(); double dy = point[1] - AutoUtils.state.getY(); @@ -131,7 +134,13 @@ public void initialize() { dt.resetDistEncs(); moveController.disable(); // output in "motor units" (arcade and tank only accept values [-1, 1] - moveController.setOutputRange(-1.0, 1.0); + LiftHeight currPos = Robot.lift.getCurrPos(); + if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) { + moveController.setOutputRange(Robot.getConst("Auto Min Speed", -0.5), + Robot.getConst("Auto Max Speed", 0.5)); + } else { + moveController.setOutputRange(-1.0, 1.0); + } moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.5)); System.out.println("move target = " + target); @@ -174,7 +183,7 @@ protected boolean isFinished() { @Override protected void end() { moveController.disable(); - System.out.println("End"); + System.out.println("PIDMove End"); // moveController.free(); double angle = Math.toRadians(AutoUtils.state.getRot()); @@ -212,6 +221,9 @@ protected void interrupted() { */ @Override public void pidWrite(double output) { + if (!hasInitialized) { + return; + } dt.arcadeDrive(output, 0); sd.putNumber("Move PID Output", output); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index c665b04..a5c0e7d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -28,6 +28,7 @@ public class PIDTurn extends Command implements PIDOutput { private double[] point; private boolean turnToPoint; private boolean absoluteRotation; + private boolean hasInitialized = false; /** * Constructs this command with a new PIDController. Sets all of the @@ -57,26 +58,18 @@ public class PIDTurn extends Command implements PIDOutput { public PIDTurn(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs, boolean absoluteRotation, boolean turnToPoint) { this.target = target; + System.out.println(this.target); this.point = point; this.dt = dt; this.ahrs = ahrs; this.sd = sd; this.turnToPoint = turnToPoint; - this.target = target; this.absoluteRotation = absoluteRotation; if (Robot.dt != null) { requires(Robot.dt); } - // calculates the maximum turning speed in degrees/sec based on the max linear - // speed in inches/s and the distance (inches) between sides of the DT - double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels()); - double r = Robot.getConst("TurnPidR", 3.0); - double kP = r / Robot.rmap.getDrivetrainTimeConstant() / maxTurnSpeed; - double kI = 0; - double kD = r / maxTurnSpeed; - double kF = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05)); - turnController = new PIDController(kP, kI, kD, kF, ahrs, this); + turnController = new PIDController(0, 0, 0, 0, ahrs, this); // tim = new Timer(); sd.putData("Turn PID", turnController); } @@ -156,6 +149,19 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s */ @Override protected void initialize() { + hasInitialized = true; + // calculate pid constants + + // max turn speed from FindTurnTimeConstant, converted to degrees + double maxTurnSpeed = Robot.getConst("Max Turn Radians Per Second", 4.0) * 180 / Math.PI; + double updateTime = sd.getConst("Default PID Update Time", 0.05); + double r = Robot.getConst("TurnPidR", 3.0); + + double kP = r / Robot.getConst("TurnTimeConstant", 0.2) / maxTurnSpeed; + double kI = 0; + double kD = r / (maxTurnSpeed * updateTime); + double kF = 1 / maxTurnSpeed * updateTime; + turnController.setPID(kP, kI, kD, kF); if (!turnToPoint) { if (absoluteRotation) { @@ -227,7 +233,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - System.out.println("isFinished"); + // System.out.println("isFinished"); return (turnController.onTarget() && Math.abs(dt.getGyroRate()) < 1); // return turnController.onTarget() // && Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When @@ -244,7 +250,7 @@ protected boolean isFinished() { @Override protected void end() { turnController.disable(); - System.out.println("end"); + System.out.println("PIDTurn end"); sd.putNumber("Turn PID Result", turnController.get()); sd.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); @@ -275,6 +281,9 @@ protected void interrupted() { */ @Override public void pidWrite(double output) { + if (!hasInitialized) { + return; + } dt.arcadeDrive(0, output); SmartDashboard.putNumber("Turn PID Output", output); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java new file mode 100644 index 0000000..b613243 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/StopIntake.java @@ -0,0 +1,41 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class StopIntake extends Command { + + private Timer tim; + + public StopIntake() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + super(); + // requires(Robot.intakeEject); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.stopIntake = true; + tim = new Timer(); + tim.reset(); + tim.start(); + } + + @Override + protected void execute() { + // Robot.intakeEject.stopIntake(); + } + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + return tim.get() > 3; + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java new file mode 100644 index 0000000..f266c32 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleIntake.java @@ -0,0 +1,23 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ToggleIntake extends InstantCommand { + + public ToggleIntake() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + super(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.intakeEject.toggleIntake(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java new file mode 100644 index 0000000..67eb5cf --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java @@ -0,0 +1,90 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.subsystems.Lift; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * This command should be the default command for the Lift subsystem + */ +public class UpdateLiftPosition extends Command { + + private Lift lift; + private double desiredDist = 0; + private LiftHeight desiredPos; + + private boolean manipulatorPluggedIn = true; + private boolean goToGround; + + public UpdateLiftPosition(Lift lift) { + requires(Robot.lift); + this.lift = lift; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // try { + // Robot.oi.manipulator.getRawAxis(1); + // } catch (NullPointerException e) { + // System.err.println("[ERROR] Manipulator not plugged in."); + // manipulatorPluggedIn = false; + // } + goToGround = false; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + // if (manipulatorPluggedIn) { + int angle = Robot.oi.manipulator.getPOV(); + + System.out.println("POV Reading: " + angle); + + if (angle == 180) { + desiredPos = LiftHeight.HOLD_CUBE; + goToGround = true; + } else if (angle == 270) { + desiredPos = LiftHeight.HOLD_CUBE; + goToGround = false; + } else if (angle != -1) { + desiredPos = LiftHeight.SWITCH; + goToGround = false; + } + + if (goToGround || angle != -1) { + desiredDist = lift.getDesiredDistFromPos(desiredPos); + lift.setSetpoint(desiredDist); + } + + if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) { + desiredPos = LiftHeight.GROUND; + desiredDist = lift.getDesiredDistFromPos(desiredPos); + lift.setSetpoint(desiredDist); + goToGround = false; + } + System.out.println("Desired Pos: " + desiredPos); + System.out.println("Desired Dist: " + desiredDist); + System.out.println("Current Dist: " + lift.getHeight()); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java index 6b6ba36..77b44d5 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java @@ -52,11 +52,13 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface { private SmartDashboardInterface sd; private boolean highGear; + private int inverted; public Drivetrain(SmartDashboardInterface sd) { this.sd = sd; highGear = false; + inverted = 1; // all 0s for controller construction because they all get set to right values // by resetAllVelocityPIDConsts @@ -85,11 +87,16 @@ public Drivetrain(SmartDashboardInterface sd) { } } + public void reverseDT() { + inverted *= -1; + } + @Override public void initDefaultCommand() { setDefaultCommand(new TeleopDrive()); } + @Override public boolean isVPIDUsed() { return Robot.getBool("Teleop velocity PID", false); } @@ -151,6 +158,7 @@ public boolean VPIDsOnTarg() { * * @return the rate of the left encoder */ + @Override public double getLeftEncRate() { return leftEncRate.getRate(); } @@ -160,6 +168,7 @@ public double getLeftEncRate() { * * @return the rate of the right encoder */ + @Override public double getRightEncRate() { return rightEncRate.getRate(); } @@ -331,6 +340,7 @@ public void resetAHRS() { fancyGyro.reset(); } + @Override public double getGyroRate() { return fancyGyro.getRate(); } @@ -432,6 +442,7 @@ public void shiftGearSolenoidOff() { /** * @return the gyroscope */ + @Override public PIDSource getGyro() { return fancyGyro; } @@ -530,6 +541,7 @@ public void putVelocityControllersToDashboard() { SmartDashboard.putData("Right PID Controller", rightVelocityController); } + @Override public double getPIDMoveConstant() { double G = Robot.rmap.getGearRatio(); double T = Robot.rmap.getStallTorque(); @@ -541,6 +553,7 @@ public double getPIDMoveConstant() { return Math.sqrt(Robot.rmap.convertMtoIn((8 * T * G) / (R * M))); } + @Override public double getPIDTurnConstant() { double G = Robot.rmap.getGearRatio(); double T = Robot.rmap.getStallTorque(); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java index 5043465..b955133 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -21,6 +21,7 @@ public class IntakeEject extends Subsystem implements IntakeEjectInterface { private final DoubleSolenoid rightSolenoid = RobotMap.rightIntakeSolenoid; private boolean leftOpen = isOpen(leftSolenoid.get()); private boolean rightOpen = isOpen(rightSolenoid.get()); + private boolean hasCube = false; /** * Return whether or not a side of the intake (L/R) is open @@ -38,6 +39,7 @@ public boolean isOpen(DoubleSolenoid.Value val) { */ @Override public void initDefaultCommand() { + // I don't want this on the manipulator joysticks during a match setDefaultCommand(new DefaultIntake()); } @@ -64,7 +66,10 @@ public double getRightIntakeSpeed() { */ @Override public boolean hasCube() { - return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 39) + SmartDashboard.putNumber("Intake Current Left", Robot.rmap.getPort("PDP Intake Left Channel", 4)); + + SmartDashboard.putNumber("Intake Current Right", Robot.rmap.getPort("PDP Intake Left Channel", 11)); + return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 15) || pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", 39); } @@ -115,6 +120,15 @@ public void runIntake(double speed) { runRightIntake(speed); } + /** + * Toggles the left and right intake between open (kReverse) and closed + * (kForward). + */ + public void toggleIntake() { + toggleLeftIntake(); + toggleRightIntake(); + } + /** * Toggles the left intake between open (kReverse) and closed (kForward). */ diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java index 4eb76db..629f133 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java @@ -2,39 +2,171 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.RobotMap; -import org.usfirst.frc.team199.Robot2018.commands.AutoLift; +import org.usfirst.frc.team199.Robot2018.commands.UpdateLiftPosition; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.command.PIDSubsystem; /** * */ -public class Lift extends Subsystem implements LiftInterface { +public class Lift extends PIDSubsystem implements LiftInterface { private final SpeedControllerGroup liftMotors = RobotMap.liftMotors; private final Encoder liftEnc = RobotMap.liftEnc; - private Position currPosition = Position.GROUND; + private LiftHeight currPosition = LiftHeight.GROUND; + + private final int NUM_STAGES; + private final double WIGGLE_ROOM; + private final double GROUND_DIST; + private final double HOLD_CUBE_DIST; + private final double SWITCH_DIST; + private final double SCALE_DIST; + private final double BAR_DIST; + + private LiftHeight desiredPos; + private boolean goToGround; + + public Lift() { + super("Lift", 0, 0, 0, 0); + + double maxSpeed = getLiftMaxSpeed(); + + System.out.println("Lift Time Const: " + getLiftTimeConstant()); + + double r = Robot.getConst("LiftPidR", 0.3); + double kP = r / getLiftTimeConstant() / maxSpeed; + double kI = 0; + double kD = r / maxSpeed; + double kF = 1 / maxSpeed * Robot.getConst("Default PID Update Time", 0.05); + + PIDController liftController = getPIDController(); + + // liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI", + // kI), Robot.getConst("LiftkD", kD), + // Robot.getConst("LiftkF", kF)); + liftController.setPID(kP, kI, kD, kF); + + setInputRange(0, Robot.getConst("Lift Max Height", 24)); + setOutputRange(-1, 1); + setAbsoluteTolerance(Robot.getConst("Lift Tolerance", 0.8)); + + // dpp = (pitch circumference of sprocket) / (pulses per rev of output shaft) + double dpp = 2 * Math.PI * getLiftRadius() / 2048; + liftEnc.setDistancePerPulse(dpp); + + NUM_STAGES = (int) Robot.getConst("Lift stages", 1); + WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches + + // calculate constant measurements + GROUND_DIST = 0; + HOLD_CUBE_DIST = 4; + // distance to switch 18.75 inches in starting position + SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES; + // distance to scale 5 feet starting 63 / 3 = 21 + SCALE_DIST = (60.0 + WIGGLE_ROOM) / NUM_STAGES; + // 7 feet starting; bar distance should be changed because I'm not aware how + // climber mech will be positioned + BAR_DIST = (84.0 + WIGGLE_ROOM) / NUM_STAGES; + enable(); + } /** * Set the default command for a subsystem here. */ + @Override public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - int angle = Robot.oi.manipulator.getPOV(); - - Command com = null; - - if (angle == 0) { - com = new AutoLift(Position.GROUND, this); - setDefaultCommand(com); - } else if (angle == 90 || angle == 180 || angle == 270) { - com = new AutoLift(Position.SWITCH, this); - setDefaultCommand(com); + setDefaultCommand(new UpdateLiftPosition(this)); + } + + /** + * @return whether or not the cube + */ + private boolean atHoldCube() { + double tol = Robot.getConst("Lift Tolerance", 0.8); + double height = getHeight(); + // true if (height within tolerance of HOLD_CUBE height) and (lift is not + // moving) + return height <= (HOLD_CUBE_DIST + tol) && height >= (HOLD_CUBE_DIST - tol) && getSpeed() <= 0.1; + } + + /** + * @return true if reached target height and does not still have to go to the + * ground as a secondary, consecutive target + */ + // @Override + // public boolean onTarget() { + // double targ = getSetpoint(); + // double tol = Robot.getConst("Lift Tolerance", 0.8); + // double height = getHeight(); + // // true if (height within tolerance of targ) and (lift is not moving) + // boolean wInTol = height <= (targ + tol) && height >= (targ - tol) && + // getSpeed() <= 0.1; + // return wInTol && !goToGround; + // } + + /** + * Set the lift's setpoint to this value continuously. + * + * @param pos + * - the desired FINAL height + * @return the FIRST height to travel to; in all cases but GROUND, FIRST = FINAL + * targ + */ + public double getDesiredDistFromPos(LiftHeight pos) { + double desiredDist; + LiftHeight height = pos; + switch (height) { + case GROUND: + // if (atHoldCube() || goToGround) { + desiredDist = GROUND_DIST; + // false bc doesn't need to hold the GROUND as a second consecutive targ anymore + goToGround = true; + break; + // } else { + // // falls through to HOLD_CUBE case + // height = LiftHeight.HOLD_CUBE; + // goToGround = false; + // } + case HOLD_CUBE: + desiredDist = HOLD_CUBE_DIST; + // goToGround = false; + break; + case SWITCH: + desiredDist = SWITCH_DIST; + // goToGround = false; + break; + case SCALE: + desiredDist = SCALE_DIST; + goToGround = false; + break; + case BAR: + desiredDist = BAR_DIST; + // goToGround = false; + break; + default: + desiredDist = GROUND_DIST; + // goToGround = false; + break; } + + return desiredDist; + } + + /** + * @return the desired LiftHeight to be at + */ + public LiftHeight desiredPos() { + return desiredPos; + } + + /** + * @return the absolute rate of the lift encoder (in/s) + */ + public double getSpeed() { + return Math.abs(liftEnc.getRate()); } /** @@ -43,20 +175,23 @@ public void initDefaultCommand() { * @param newPosition * - the new position meant to be set */ - public void setCurrPosition(Position newPosition) { + @Override + public void setCurrPosition(LiftHeight newPosition) { currPosition = newPosition; } /** - * Uses (insert sensor here) to detect the current lift position + * Uses AMT103 Encoder to detect the current lift position */ + @Override public double getHeight() { - return liftEnc.getDistance() * 3; + return liftEnc.getDistance() * NUM_STAGES; } /** * stops the lift */ + @Override public void stopLift() { liftMotors.stopMotor(); } @@ -64,18 +199,25 @@ public void stopLift() { /** * gets current motor values */ + @Override public double getLiftSpeed() { return liftMotors.get(); } /** - * Runs lift motors at specified speed + * Sends specific voltage to lift motor, clamped at max voltage * - * @param speed - * - desired speed to run at + * @param output + * - desired voltage to set motor to [-1, 1] */ + @Override public void runMotor(double output) { - liftMotors.set(output); + double absMax = Robot.getConst("Lift Max Voltage", 0.5); + double out = output; + if (Math.abs(out) > absMax) { + out = Math.signum(out) * absMax; + } + liftMotors.set(out); } /** @@ -83,15 +225,109 @@ public void runMotor(double output) { * * @return pos - current position */ - public Position getCurrPos() { + @Override + public LiftHeight getCurrPos() { return currPosition; } /** * Resets the encoder */ + @Override public void resetEnc() { liftEnc.reset(); } + /** + * Gets the number of stages variable + */ + @Override + public int getNumStages() { + return NUM_STAGES; + } + + /** + * Gets the extra distance above the switch or scale we want to lift in inches + */ + @Override + public double getWiggleRoom() { + return WIGGLE_ROOM; + } + + /** + * @return the max speed of the lift (in/s) + */ + public double getLiftMaxSpeed() { + double maxSpd = RobotMap.getOmegaMax() / getLiftGearRatio() / 60 * 2 * Math.PI * getLiftRadius(); + return Robot.getConst("Lift Max Speed", maxSpd); + } + + /** + * Uses AMT103 Encoder to detect the current lift height with respect to the + * lift's min height (inches) + */ + @Override + protected double returnPIDInput() { + // return getHeight(); //Use this instead? What's the difference? + return getHeight(); + } + + /** + * Runs the lift motors at the value that the pid loop calculated. Used + * internally by PIDSubsystem. + */ + @Override + protected void usePIDOutput(double output) { + double out = output; + double spd = liftEnc.getRate(); + out += Robot.getConst("Lift: Necessary Voltage", 0.25); + runMotor(out); + } + + /** + * Gets the time constant of the drivetrain, which is used to calculate PID + * constants. + * + * @return time constant + */ + public double getLiftTimeConstant() { + double gearReduction = getLiftGearRatio(); + double radius = convertInToM(getLiftRadius()); + double timeConstant = RobotMap.getOmegaMax() / gearReduction / 60 * 2 * Math.PI + * RobotMap.convertNtokG(getLiftedWeight()) * radius * radius + / (RobotMap.getStallTorque() * gearReduction); + return timeConstant; + } + + /** + * @param inches + * - an amount in inches + * @return the equivalent amount in meters + */ + public double convertInToM(double inches) { + return inches * 2.54 / 100; + } + + /** + * @return the gear ratio of the lift gearbox + */ + public double getLiftGearRatio() { + return Robot.getConst("Lift Gear Reduction", 10); + } + + /** + * @return the lift sprocket's radius (inches) + */ + public double getLiftRadius() { + // 15 tooth, #35 sprocket + return Robot.getConst("Lift Sprocket Pitch Diam", 1.79) / 2; + } + + /** + * @return the weight (in Newtons) of everything that is being lifted: intake, + * lift components, NOT cube + */ + public double getLiftedWeight() { + return Robot.getConst("Weight of Lifted Stuff", 62.91); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java index 8ba11d4..917b820 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java @@ -1,58 +1,91 @@ package org.usfirst.frc.team199.Robot2018.subsystems; -import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position; - public interface LiftInterface { - + /** * Set the default command for a subsystem here. - * */ + */ public void initDefaultCommand(); - - public enum Position { - GROUND, - SWITCH, - SCALE, - BAR + + public enum LiftHeight { + GROUND, HOLD_CUBE, SWITCH, SCALE, BAR; + + public static LiftHeight toLH(String str) { + LiftHeight lh = null; + switch (str) { + case "GROUND": + lh = GROUND; + break; + case "HOLD_CUBE": + lh = HOLD_CUBE; + break; + case "SWITCH": + lh = SWITCH; + break; + case "SCALE": + lh = SCALE; + break; + case "BAR": + lh = BAR; + break; + default: + lh = GROUND; + break; + } + return lh; + } } - + /** - * Uses (insert sensor here) to detect the current lift position + * Uses (insert sensor here) to detect the current lift position */ public double getHeight(); - + /** * stops the lift */ public void stopLift(); - + /** * gets current motor values */ public double getLiftSpeed(); - + /** * Runs lift motors at specified speed - * @param speed - desired speed to run at + * + * @param speed + * - desired speed to run at */ public void runMotor(double speed); - + /** * Returns the position the lift is currently at + * * @return pos - current position */ - public Position getCurrPos(); - + public LiftHeight getCurrPos(); + /** * Resets the encoder */ public void resetEnc(); - + /** * Sets the current position in the lift subsystem - * @param newPosition - the new position meant to be set + * + * @param newPosition + * - the new position meant to be set + */ + public void setCurrPosition(LiftHeight newPosition); + + /** + * Gets the number of stages variable + */ + public int getNumStages(); + + /** + * Gets the extra distance above the switch or scale we want to lift in inches */ - public void setCurrPosition(Position newPosition); - - + public double getWiggleRoom(); } diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java index 6359c69..8e96322 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java @@ -1,18 +1,13 @@ package org.usfirst.frc.team199.Robot2018; -import org.junit.jupiter.api.*; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.junit.runner.RunWith; -import org.mockito.invocation.InvocationOnMock; -import org.mockito.stubbing.Answer; - import static org.junit.Assert.assertTrue; import static org.mockito.Mockito.mock; import static org.mockito.Mockito.when; -import java.util.Arrays; -import java.util.Collection; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.mockito.invocation.InvocationOnMock; +import org.mockito.stubbing.Answer; import edu.wpi.first.wpilibj.HLUsageReporting; import edu.wpi.first.wpilibj.PIDController; @@ -21,32 +16,29 @@ import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.internal.HardwareTimer; -import org.junit.runners.Parameterized; -import org.junit.runners.Parameterized.Parameters; -import org.junit.runners.Parameterized.Parameter; //@RunWith(value = Parameterized.class) class PIDConstSim { static final double initkP = 10; static final double initkI = 6; static final double initkD = 6; - - //@Parameter(value = ) + + // @Parameter(value = ) double liftkP = initkP; double liftkI = initkI; double liftkD = initkD; - + private final double targetOne = 24; private final double targetTwo = 60; private final double targetThree = 45; // 1 in in meters private final double tolerance = 0.0254; - + double position = 0; double velocity_ = 0; double voltage_ = 0; double offset_ = -0.1; - + // Stall Torque in N m static final double kStallTorque = 2.41; // Stall Current in Amps @@ -63,8 +55,7 @@ class PIDConstSim { // Resistance of the motor static final double kResistance = 12.0 / kStallCurrent; // Motor velocity constant - static final double Kv = ((kFreeSpeed / 60.0 * 2.0 * Math.PI) / - (12.0 - kResistance * kFreeCurrent)); + static final double Kv = ((kFreeSpeed / 60.0 * 2.0 * Math.PI) / (12.0 - kResistance * kFreeCurrent)); // Torque constant static final double Kt = (kNumMotors * kStallTorque) / kStallCurrent; // Gear ratio @@ -75,61 +66,46 @@ class PIDConstSim { // Control loop time step static final double kDt = 0.01; - + double current_time = 0; - - // V = I * R + omega / Kv - // torque = Kt * I - - + + // V = I * R + omega / Kv + // torque = Kt * I + // (name = "{index}: testAdd({0}+{1}) = {2}") - /*@Parameters - public static double[][] data() { - // array is P I and D finalants in order - //double [][][] PIDfinalTable = new double[5][5][5]; - double [][] PIDfinalTable = new double[3][5]; - double diffInter3 = initkD / 10; - double diffInter2 = initkI / 10; - double diffInter1 = initkP / 10; - for(int i = 0; i < 3; i++) { - for(int j = -2; j < 3; j++) { - double res; - if(i == 0) - res = initkP + j * diffInter1; - else if(i == 1) - res = initkI + j * diffInter2; - else - res = initkD + j * diffInter3; - PIDfinalTable[i][j] = res; - } - } - //return (Iterable)Arrays.asList(PIDfinalTable); - return PIDfinalTable; - } - */ - + /* + * @Parameters public static double[][] data() { // array is P I and D finalants + * in order //double [][][] PIDfinalTable = new double[5][5][5]; double [][] + * PIDfinalTable = new double[3][5]; double diffInter3 = initkD / 10; double + * diffInter2 = initkI / 10; double diffInter1 = initkP / 10; for(int i = 0; i < + * 3; i++) { for(int j = -2; j < 3; j++) { double res; if(i == 0) res = initkP + + * j * diffInter1; else if(i == 1) res = initkI + j * diffInter2; else res = + * initkD + j * diffInter3; PIDfinalTable[i][j] = res; } } //return + * (Iterable)Arrays.asList(PIDfinalTable); return PIDfinalTable; } + */ + private double GetAcceleration(double voltage) { - return -Kt * kG * kG / (Kv * kResistance * kr * kr * kMass) * velocity_ + - kG * Kt / (kResistance * kr * kMass) * voltage; - } - + return -Kt * kG * kG / (Kv * kResistance * kr * kr * kMass) * velocity_ + + kG * Kt / (kResistance * kr * kMass) * voltage; + } + private void simulateTime(double time, double voltage) { final double starting_time = time; - while (time > 0) { - final double current_dt = Math.min(time, 0.001); - //System.out.println("vel before: " + velocity_); - position += current_dt * velocity_; - double acc = GetAcceleration(voltage); - //System.out.println("acc: " + acc); - velocity_ += current_dt * acc; - //System.out.println("vel after: " + velocity_); - time -= 0.001; - //EXPECT_LE(position, ElevatorLoop::kMaxHeight + 0.01); - //EXPECT_GE(position, ElevatorLoop::kMinHeight - 0.01); - } - current_time += starting_time; + while (time > 0) { + final double current_dt = Math.min(time, 0.001); + // System.out.println("vel before: " + velocity_); + position += current_dt * velocity_; + double acc = GetAcceleration(voltage); + // System.out.println("acc: " + acc); + velocity_ += current_dt * acc; + // System.out.println("vel after: " + velocity_); + time -= 0.001; + // EXPECT_LE(position, ElevatorLoop::kMaxHeight + 0.01); + // EXPECT_GE(position, ElevatorLoop::kMinHeight - 0.01); + } + current_time += starting_time; } - + @BeforeEach void setUp() { // Since VelocityPIDController extends PIDController and PIDController calls @@ -140,50 +116,50 @@ void setUp() { HardwareTimer tim = mock(HardwareTimer.class); Timer.Interface timerInstance = mock(Timer.Interface.class); when(tim.newTimer()).thenReturn(timerInstance); - when(timerInstance.get()).thenAnswer( - new Answer() { - public Object answer(InvocationOnMock invocation) { - //return getTime(); - return current_time; - } - }); - + when(timerInstance.get()).thenAnswer(new Answer() { + public Object answer(InvocationOnMock invocation) { + // return getTime(); + return current_time; + } + }); + Timer.SetImplementation(tim); HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class); HLUsageReporting.SetImplementation(usageReporter); } - - class PIDSim{ + + class PIDSim { public PIDSim() { - //position = 0; + // position = 0; } - + public double pidGet() { return position; } - + public void pidWrite(double output) { - //System.out.println("voltage: " + output); + // System.out.println("voltage: " + output); simulateTime(kDt, output * 12); System.out.println("position: " + position); } } - - class PIDSimController extends PIDController{ - + + class PIDSimController extends PIDController { + public PIDSimController(double p, double i, double d, PIDSource src, PIDOutput out) { - super(p,i,d,src,out); + super(p, i, d, src, out); } - + public void calc() { this.calculate(); } - + } - - class PIDSimSource implements PIDSource{ + + class PIDSimSource implements PIDSource { private PIDSim sim; private PIDSourceType type; + public PIDSimSource(PIDSim sim) { this.sim = sim; } @@ -197,25 +173,28 @@ public PIDSourceType getPIDSourceType() { public double pidGet() { return sim.pidGet(); } - + @Override public void setPIDSourceType(PIDSourceType pidSource) { // TODO Auto-generated method stub type = pidSource; } } - - class PIDSimOutput implements PIDOutput{ + + class PIDSimOutput implements PIDOutput { private PIDSim sim; + public PIDSimOutput(PIDSim sim) { this.sim = sim; } + @Override public void pidWrite(double output) { sim.pidWrite(output); } } + @Test void test24() { PIDSim sim = new PIDSim(); PIDSimSource src = new PIDSimSource(sim); @@ -225,8 +204,8 @@ void test24() { liftController.setSetpoint(0.6); liftController.setOutputRange(-1.0, 1.0); liftController.enable(); - //1 second? - for(int i = 0; i < 100; i++) { + // 1 second? + for (int i = 0; i < 100; i++) { liftController.calc(); } System.out.println("Test" + targetOne + ": " + sim.pidGet()); @@ -235,6 +214,7 @@ void test24() { assertTrue(Math.abs(0.6 - sim.pidGet()) < tolerance); } + @Test void test60() { PIDSim sim = new PIDSim(); PIDSimSource src = new PIDSimSource(sim); @@ -244,8 +224,8 @@ void test60() { liftController.setSetpoint(1.524); liftController.setOutputRange(-1.0, 1.0); liftController.enable(); - //1 second? - for(int i = 0; i < 200; i++) { + // 1 second? + for (int i = 0; i < 200; i++) { liftController.calc(); } System.out.println("" + sim.pidGet()); @@ -263,8 +243,8 @@ void test45() { liftController.setSetpoint(1.143); liftController.setOutputRange(-1.0, 1.0); liftController.enable(); - //1 second? - for(int i = 0; i < 100; i++) { + // 1 second? + for (int i = 0; i < 100; i++) { liftController.calc(); } System.out.println("" + sim.pidGet()); diff --git a/docs/controllers.txt b/docs/controllers.txt index 364e3c3..50ed1de 100644 --- a/docs/controllers.txt +++ b/docs/controllers.txt @@ -2,9 +2,9 @@ buttons 1 slow forward-backward movement in arcade, or all movement in tank 2 toggle drive type (arcade/tank) [ShiftDriveType] - 3 - 4 - 5 + 3 move lift up w/ incremental PID [MoveLiftWithPID...true] + 4 move lift down w/ incremental PID [MoveLiftWithPID...false] + 5 use to test PID, targ height changed in SD [LiftToPosition] 6 7 move with PID by inches from SmartDashboard "Move Targ" [PIDMove] 8 turn with PID by degrees from SmartDashboard "Turn Targ" [PIDTurn] @@ -26,8 +26,8 @@ 7 8 update the PIDController constants with SmartDashboard values [UpdatePIDConstants] 9 set the left and right encoder distance per pulse with SmartDashboard values [SetDistancePerPulse] - 10 - 11 + 10 move lift up while held at very slow speed [MoveLift...true] + 11 move lift down while held at very slow speed [MoveLift...false] axes 0 X axis arcade turn (if default) 1 Y axis tank right, if NOT default: arcade forward diff --git a/shuffleboard.json b/shuffleboard.json index d6775cd..e69de29 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -1,946 +0,0 @@ -{ - "dividerPosition": 0.28486646884273, - "tabPane": [ - { - "title": "PID Testing", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "0,0": { - "size": [ - 1, - 2 - ], - "content": { - "_type": "PID Controller", - "_source0": "network_table:///SmartDashboard/Left PID Controller", - "_title": "SmartDashboard/Left PID Controller" - } - }, - "1,0": { - "size": [ - 1, - 2 - ], - "content": { - "_type": "PID Controller", - "_source0": "network_table:///SmartDashboard/Right PID Controller", - "_title": "SmartDashboard/Right PID Controller" - } - }, - "0,2": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Left Enc Rate", - "_title": "/SmartDashboard/Left Enc Rate", - "Visible time": 30.0 - } - }, - "5,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Left VPID Error", - "_title": "SmartDashboard/Left VPID Error" - } - }, - "2,2": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Right Enc Rate", - "_title": "/SmartDashboard/Right Enc Rate", - "Visible time": 30.0 - } - }, - "5,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Right VPID Error", - "_title": "SmartDashboard/Right VPID Error" - } - }, - "2,0": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", - "_title": "SmartDashboard/Drivetrain/Left VPID Targ", - "Visible time": 30.0, - "SmartDashboard/Drivetrain/Left VPID Targ visible": true - } - }, - "4,0": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", - "_title": "SmartDashboard/Drivetrain/Right VPID Targ", - "Visible time": 30.0, - "SmartDashboard/Drivetrain/Right VPID Targ visible": true - } - }, - "4,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Const/VelocityRightkI", - "_title": "SmartDashboard/Const/VelocityRightkI" - } - }, - "4,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Const/VelocityLeftkI", - "_title": "SmartDashboard/Const/VelocityLeftkI" - } - } - } - } - }, - { - "title": "Drivetrain", - "autoPopulate": true, - "autoPopulatePrefix": "SmartDashboard/Drivetrain", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "0,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", - "_title": "Left VPID Targ" - } - }, - "1,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", - "_title": "Right VPID Targ" - } - }, - "2,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Current Max Speed", - "_title": "Current Max Speed" - } - }, - "3,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Dist", - "_title": "Left Enc Dist" - } - }, - "4,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Rate", - "_title": "Left Enc Rate" - } - }, - "5,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Dist", - "_title": "Right Enc Dist" - } - }, - "0,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Rate", - "_title": "Right Enc Rate" - } - }, - "1,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Drivetrain/Enc Avg Dist", - "_title": "Enc Avg Dist" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/Bool/High Gear", - "_title": "SmartDashboard/Bool/High Gear", - "colorWhenTrue": "#7CFC00FF", - "colorWhenFalse": "#8B0000FF" - } - }, - "2,1": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Toggle Switch", - "_source0": "network_table:///Preferences/Bool/Square Drive Values", - "_title": "Preferences/Bool/Square Drive Values" - } - }, - "2,2": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Toggle Switch", - "_source0": "network_table:///Preferences/Bool/Square Joystick Values", - "_title": "Preferences/Bool/Square Joystick Values" - } - }, - "1,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Toggle Switch", - "_source0": "network_table:///Preferences/Bool/Teleop velocity PID", - "_title": "Preferences/Bool/Teleop velocity PID" - } - } - } - } - }, - { - "title": "Move PID Testing", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "3,0": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Right Enc Dist", - "_title": "/SmartDashboard/Right Enc Dist", - "Visible time": 10.0 - } - }, - "1,0": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Left Enc Dist", - "_title": "/SmartDashboard/Left Enc Dist", - "Visible time": 10.0 - } - }, - "5,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Network Table Tree", - "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", - "_title": "LiveWindow/Ungrouped/Scheduler" - } - }, - "0,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Move PID Output", - "_title": "SmartDashboard/Move PID Output" - } - }, - "1,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Move PID Result", - "_title": "SmartDashboard/Move PID Result" - } - }, - "1,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Move PID Error", - "_title": "SmartDashboard/Move PID Error" - } - }, - "2,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Right VPID Error", - "_title": "SmartDashboard/Right VPID Error" - } - }, - "2,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Left VPID Error", - "_title": "SmartDashboard/Left VPID Error" - } - }, - "3,2": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", - "_title": "SmartDashboard/Drivetrain/Left VPID Targ", - "Visible time": 10.0, - "SmartDashboard/Drivetrain/Left VPID Targ visible": true - } - }, - "5,2": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "Graph", - "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", - "_title": "SmartDashboard/Drivetrain/Right VPID Targ", - "Visible time": 10.0, - "SmartDashboard/Drivetrain/Right VPID Targ visible": true - } - }, - "5,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Const/MoveTolerance", - "_title": "SmartDashboard/Const/MoveTolerance" - } - }, - "0,0": { - "size": [ - 1, - 2 - ], - "content": { - "_type": "PID Controller", - "_source0": "network_table:///SmartDashboard/Move PID", - "_title": "SmartDashboard/Move PID" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Move Targ", - "_title": "Preferences/Const/Move Targ" - } - } - } - } - }, - { - "title": "Turn PID Testing", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "2,0": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Network Table Tree", - "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", - "_title": "LiveWindow/Ungrouped/Scheduler" - } - }, - "3,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Turn PID Error", - "_title": "SmartDashboard/Turn PID Error" - } - }, - "4,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Turn PID Output", - "_title": "SmartDashboard/Turn PID Output" - } - }, - "2,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Turn PID Result", - "_title": "SmartDashboard/Turn PID Result" - } - }, - "0,2": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Angle", - "_title": "SmartDashboard/Angle" - } - }, - "0,0": { - "size": [ - 2, - 2 - ], - "content": { - "_type": "PID Controller", - "_source0": "network_table:///SmartDashboard/Turn PID", - "_title": "SmartDashboard/Turn PID" - } - }, - "4,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Left VPID Error", - "_title": "SmartDashboard/Left VPID Error" - } - }, - "4,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Right VPID Error", - "_title": "SmartDashboard/Right VPID Error" - } - }, - "4,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Const/Turn Targ", - "_title": "SmartDashboard/Const/Turn Targ" - } - }, - "3,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Const/TurnTolerance", - "_title": "SmartDashboard/Const/TurnTolerance" - } - }, - "2,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Output", - "_title": "Preferences/Const/Output" - } - } - } - } - }, - { - "title": "PID Constants", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "0,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/TurnkD", - "_title": "Preferences/Const/TurnkD" - } - }, - "0,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/TurnkI", - "_title": "Preferences/Const/TurnkI" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/TurnkP", - "_title": "Preferences/Const/TurnkP" - } - }, - "4,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Output", - "_title": "Preferences/Const/Output" - } - }, - "2,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/MovekP", - "_title": "Preferences/Const/MovekP" - } - }, - "2,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/MovekI", - "_title": "Preferences/Const/MovekI" - } - }, - "2,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/MovekD", - "_title": "/Preferences/Const/MovekD" - } - }, - "2,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/MoveTolerance", - "_title": "Preferences/Const/MoveTolerance" - } - }, - "0,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/TurnTolerance", - "_title": "Preferences/Const/TurnTolerance" - } - }, - "4,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Weight of Robot", - "_title": "Preferences/Const/Weight of Robot" - } - } - } - } - }, - { - "title": "Autonomous", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "0,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/Starting Position", - "_title": "SmartDashboard/Starting Position" - } - }, - "0,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/LL", - "_title": "SmartDashboard/LL" - } - }, - "1,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/LR", - "_title": "SmartDashboard/LR" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/RL", - "_title": "SmartDashboard/RL" - } - }, - "1,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/RR", - "_title": "SmartDashboard/RR" - } - }, - "1,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Auto Delay", - "_title": "SmartDashboard/Auto Delay" - } - }, - "2,0": { - "size": [ - 3, - 2 - ], - "content": { - "_type": "Network Table Tree", - "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", - "_title": "LiveWindow/Ungrouped/Scheduler" - } - }, - "2,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/MoveTolerance", - "_title": "Preferences/Const/MoveTolerance" - } - }, - "0,3": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Stall Torque", - "_title": "Preferences/Const/Stall Torque" - } - }, - "1,3": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/PID Move Fudge Factor", - "_title": "Preferences/Const/PID Move Fudge Factor" - } - } - } - } - }, - { - "title": "VPID Testing 2", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "tiles": { - "0,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityLeftkD", - "_title": "Preferences/Const/VelocityLeftkD" - } - }, - "0,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityLeftkI", - "_title": "Preferences/Const/VelocityLeftkI" - } - }, - "1,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityRightkD", - "_title": "Preferences/Const/VelocityRightkD" - } - }, - "1,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityRightkI", - "_title": "Preferences/Const/VelocityRightkI" - } - }, - "2,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VPID Test Set", - "_title": "Preferences/Const/VPID Test Set" - } - }, - "3,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/kTimeoutMs", - "_title": "Preferences/Const/kTimeoutMs" - } - }, - "3,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/Motor Deadband", - "_title": "Preferences/Const/Motor Deadband" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityLeftkF", - "_title": "Preferences/Const/VelocityLeftkF" - } - }, - "1,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///Preferences/Const/VelocityRightkF", - "_title": "Preferences/Const/VelocityRightkF" - } - }, - "3,2": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Time Const (kD)", - "_title": "SmartDashboard/Time Const (kD)" - } - } - } - } - } - ] -} \ No newline at end of file