From 5bfb1c52b8014fee1028c6b37a22b1bc683989c6 Mon Sep 17 00:00:00 2001 From: nelsonychs Date: Sun, 18 Feb 2018 22:54:42 -0800 Subject: [PATCH 1/7] Added auto testing code and other minor fixes --- .../frc/team199/Robot2018/AutoLiftTest.java | 179 ++++++++++++++++++ 1 file changed, 179 insertions(+) create mode 100644 Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoLiftTest.java diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoLiftTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoLiftTest.java new file mode 100644 index 0000000..2df270d --- /dev/null +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoLiftTest.java @@ -0,0 +1,179 @@ +package org.usfirst.frc.team199.Robot2018; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.mockito.invocation.InvocationOnMock; +import org.mockito.stubbing.Answer; + +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.when; + +import edu.wpi.first.wpilibj.HLUsageReporting; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.internal.HardwareTimer; + +class AutoLiftTest { + + final double liftkP = 0.01; + final double liftkI = 0.0001; + final double liftkD = 0.002; + private final double targetOne = 24; + private final double targetTwo = 60; + private final double targetThree = 45; + private int iterations; + private final double tolerance = 1; + + @BeforeEach + void setUp() { + // Since VelocityPIDController extends PIDController and PIDController calls + // static methods in wpilib that only work on robot, + // we setup these mocks to allow the tests to run off robot. + iterations = 0; + 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(); + } + }); + + Timer.SetImplementation(tim); + HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class); + HLUsageReporting.SetImplementation(usageReporter); + } + + private double getTime() { + iterations++; + return iterations * 0.02; + } + + class PIDSim{ + private double totalDist = 0; + private final double distPerPulse = 0.01; + // 1/pulses per rev, 1/99? + private final double circum = Math.PI * 6; + public PIDSim() { + totalDist = 0; + } + + public double pidGet() { + return totalDist; + } + + public void pidWrite(double output) { + //256 rpm + totalDist += (output * 256) * distPerPulse * circum; + } + } + + class PIDSimController extends PIDController{ + + public PIDSimController(double p, double i, double d, PIDSource src, PIDOutput out) { + super(p,i,d,src,out); + } + + public void calc() { + this.calculate(); + } + + } + + class PIDSimSource implements PIDSource{ + private PIDSim sim; + private PIDSourceType type; + public PIDSimSource(PIDSim sim) { + this.sim = sim; + } + + @Override + public PIDSourceType getPIDSourceType() { + return type; + } + + @Override + public double pidGet() { + return sim.pidGet(); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + // TODO Auto-generated method stub + type = pidSource; + } + } + + 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); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(targetOne); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 50; i++) { + liftController.calc(); + } + System.out.println("" + sim.pidGet()); + assert(sim.pidGet() > 0); + assert(Math.abs(targetOne - sim.pidGet()) < tolerance); + } + + @Test + void test60() { + PIDSim sim = new PIDSim(); + PIDSimSource src = new PIDSimSource(sim); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(targetTwo); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 50; i++) { + liftController.calc(); + } + System.out.println("" + sim.pidGet()); + assert(sim.pidGet() > 0); + assert(Math.abs(targetTwo - sim.pidGet()) < tolerance); + } + + @Test + void test45() { + PIDSim sim = new PIDSim(); + PIDSimSource src = new PIDSimSource(sim); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(targetThree); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 50; i++) { + liftController.calc(); + } + System.out.println("" + sim.pidGet()); + assert(sim.pidGet() > 0); + assert(Math.abs(targetThree - sim.pidGet()) < tolerance); + } + +} From 814628a193012fb1d8e8d975521a1443aa9408f2 Mon Sep 17 00:00:00 2001 From: Gabe Mitnick Date: Mon, 19 Feb 2018 12:10:18 -0800 Subject: [PATCH 2/7] add JavaDocs to some commands and fix naming for example, outake -> outtake, adding "Button" to button names, and ensuring they all start lowercase --- .../org/usfirst/frc/team199/Robot2018/OI.java | 62 +++++++++---------- .../{OutakeCube.java => OuttakeCube.java} | 4 +- .../Robot2018/commands/ResetEncoders.java | 2 +- .../commands/SetDistancePerPulse.java | 3 +- .../commands/UpdatePIDConstants.java | 2 +- 5 files changed, 37 insertions(+), 36 deletions(-) rename Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/{OutakeCube.java => OuttakeCube.java} (93%) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 51a2551..3c1003a 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -11,7 +11,7 @@ import org.usfirst.frc.team199.Robot2018.commands.IntakeCube; import org.usfirst.frc.team199.Robot2018.commands.LowerIntake; import org.usfirst.frc.team199.Robot2018.commands.OpenIntake; -import org.usfirst.frc.team199.Robot2018.commands.OutakeCube; +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.RaiseIntake; @@ -41,21 +41,21 @@ public class OI { private JoystickButton shiftLowGearButton; private JoystickButton shiftHighGearButton; private JoystickButton shiftDriveTypeButton; - private JoystickButton PIDMoveButton; - private JoystickButton PIDTurnButton; + private JoystickButton pIDMoveButton; + private JoystickButton pIDTurnButton; private JoystickButton resetEncButton; - private JoystickButton MoveLiftUpButton; - private JoystickButton MoveLiftDownButton; + private JoystickButton moveLiftUpButton; + private JoystickButton moveLiftDownButton; public Joystick rightJoy; private JoystickButton updatePIDConstantsButton; private JoystickButton updateEncoderDPPButton; public Joystick manipulator; - private JoystickButton closeIntake; - private JoystickButton openIntake; - private JoystickButton raiseIntake; - private JoystickButton lowerIntake; - private JoystickButton intake; - private JoystickButton outake; + private JoystickButton closeIntakeButton; + private JoystickButton openIntakeButton; + private JoystickButton raiseIntakeButton; + private JoystickButton lowerIntakeButton; + private JoystickButton intakeCubeButton; + private JoystickButton outtakeCubeButton; public int getButton(String key, int def) { if (!SmartDashboard.containsKey("Button/" + key)) { @@ -71,13 +71,13 @@ public OI() { leftJoy = new Joystick(0); shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2)); shiftDriveTypeButton.whenPressed(new ShiftDriveType()); - PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7)); - PIDMoveButton + pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7)); + pIDMoveButton .whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg)); - PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8)); + pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8)); // PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90), // Robot.dt, Robot.sd RobotMap.fancyGyro)); - PIDTurnButton + pIDTurnButton .whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro)); resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10)); resetEncButton.whenPressed(new ResetEncoders()); @@ -91,23 +91,23 @@ public OI() { updatePIDConstantsButton.whenPressed(new UpdatePIDConstants()); updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9)); updateEncoderDPPButton.whenPressed(new SetDistancePerPulse()); - MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10)); - MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11)); - MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); - MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); + moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10)); + moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11)); + moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); + moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); manipulator = new Joystick(2); - closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1)); - closeIntake.whenPressed(new CloseIntake()); - openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2)); - openIntake.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()); - intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); - intake.whenPressed(new IntakeCube()); - outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); - outake.whenPressed(new OutakeCube()); + 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()); + raiseIntakeButton = new JoystickButton(manipulator, getButton("Raise Intake Button", 3)); + raiseIntakeButton.whenPressed(new RaiseIntake()); + lowerIntakeButton = new JoystickButton(manipulator, getButton("Lower Intake Button", 4)); + lowerIntakeButton.whenPressed(new LowerIntake()); + intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5)); + intakeCubeButton.whenPressed(new IntakeCube()); + outtakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6)); + outtakeCubeButton.whenPressed(new OuttakeCube()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java similarity index 93% rename from Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java rename to Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java index e68dea4..a3cc853 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java @@ -8,11 +8,11 @@ /** * */ -public class OutakeCube extends Command { +public class OuttakeCube extends Command { Timer tim; - public OutakeCube() { + public OuttakeCube() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java index 758068d..3c4ef57 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.command.InstantCommand; /** - * + * Resets left and right drivetrain encoders to 0 */ public class ResetEncoders extends InstantCommand { diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java index 597a0bb..2b01556 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java @@ -5,7 +5,8 @@ import edu.wpi.first.wpilibj.command.InstantCommand; /** - * + * Sets distance per pulse for left and right drivetrain encoders from + * SmartDashboard "Distance Per Pulse Left" & "Distance Per Pulse Right" */ public class SetDistancePerPulse extends InstantCommand { diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java index 1748df3..febf71d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.command.InstantCommand; /** - * + * Updates the PIDControllers' PIDConstants based on SmartDashboard values */ public class UpdatePIDConstants extends InstantCommand { From 1879b6b0159d36640be01abb32316d69f21789d5 Mon Sep 17 00:00:00 2001 From: Gabe Mitnick Date: Mon, 19 Feb 2018 12:13:27 -0800 Subject: [PATCH 3/7] update controllers.txt with all of the buttons in OI --- docs/controllers.txt | 74 ++++++++++++++++++++++---------------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/docs/controllers.txt b/docs/controllers.txt index 6cd1c8c..44ed967 100644 --- a/docs/controllers.txt +++ b/docs/controllers.txt @@ -1,60 +1,60 @@ controller indices - manipulator - buttons - 1 A - 2 B - 3 X - 4 Y - 5 LB - 6 RB - 7 back - 8 start - 9 N/a - 10 N/a - press mode to toggle mode light - axes - 0 left thumbstick X axis (D-pad if mode light on) - 1 left thumbstick Y axis (D-pad if mode light on) - 2 left trigger - 3 right trigger - 4 right thumbstick X axis - 5 right thumbstick Y axis - mode light off: - POV - D-pad (left joystick if mode light on) - rumble - fake news - left joystick + left joystick [0] buttons 1 - 2 + 2 toggle drive type (arcade/tank) [ShiftDriveType] 3 4 5 6 - 7 - 8 + 7 move with PID by inches from SmartDashboard "Move Targ" [PIDMove] + 8 turn with PID by degrees from SmartDashboard "Turn Targ" [PIDTurn] 9 - 10 + 10 reset left and right drivetrain encoders to 0 [ResetEncoders] 11 axes 0 X axis 1 Y axis (forward is negative) 2 Z axis (knob at base) - right joystick + right joystick [1] buttons 1 - 2 - 3 + 2 shift drivetrain to low gear [ShiftLowGear] + 3 shift drivetrain to high gear [ShiftHighGear] 4 5 6 7 - 8 - 9 - 10 - 11 + 8 update the PIDController constants with SmartDashboard values [UpdatePIDConstants] + 9 set the left and right encoder distance per pulse with SmartDashboard values [SetDistancePerPulse] + 10 run lift motor up [RunLift] + 11 run lift motor down [RunLift] axes 0 X axis 1 Y axis (forward is negative) 2 Z axis (knob at base) + manipulator [3] + buttons + 1 A close intake [CloseIntake] + 2 B open intake [OpenIntake] + 3 X raise intake [RaiseIntake] + 4 Y lower intake [LowerIntake] + 5 LB take in [IntakeCube] + 6 RB take out [OuttakeCube] + 7 back + 8 start + 9 N/a + 10 N/a + press mode to toggle mode light + axes + 0 left thumbstick X axis (D-pad if mode light on) + 1 left thumbstick Y axis (D-pad if mode light on) + 2 left trigger + 3 right trigger + 4 right thumbstick X axis + 5 right thumbstick Y axis + mode light off: + POV + D-pad (left joystick if mode light on) + rumble + fake news From f2a2becfe12623e881d110287b68ac14af5b18e1 Mon Sep 17 00:00:00 2001 From: nelsonychs Date: Mon, 19 Feb 2018 12:24:10 -0800 Subject: [PATCH 4/7] Mapped 4 stages of lift to manipulator POV for tele, also minor tweaks The 4 stages of lift are now mapped to POV for teleop. Also did tweaks like motor ports, slaves, etc. Also note that manipulator in OI is static now for default command in lift. --- .../org/usfirst/frc/team199/Robot2018/OI.java | 11 +++-- .../frc/team199/Robot2018/RobotMap.java | 20 +++++--- .../team199/Robot2018/commands/AutoLift.java | 38 +++++---------- .../team199/Robot2018/subsystems/Climber.java | 4 +- .../team199/Robot2018/subsystems/Lift.java | 48 ++++++++++++------- .../Robot2018/subsystems/LiftInterface.java | 14 ++---- .../frc/team199/Robot2018/LiftTester.java | 0 7 files changed, 66 insertions(+), 69 deletions(-) delete mode 100644 Robot2018/test/org/usfirst/frc/team199/Robot2018/LiftTester.java diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 7e4e616..49c4b81 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -43,7 +43,7 @@ public class OI { public Joystick rightJoy; private JoystickButton updatePIDConstantsButton; private JoystickButton updateEncoderDPPButton; - public Joystick manipulator; + public static Joystick manipulator; private JoystickButton closeIntake; private JoystickButton openIntake; private JoystickButton raiseIntake; @@ -85,10 +85,6 @@ public OI() { updatePIDConstantsButton.whenPressed(new UpdatePIDConstants()); updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9)); updateEncoderDPPButton.whenPressed(new SetDistancePerPulse()); - MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10)); - MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11)); - MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); - MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); // manipulator = new Joystick(2); // closeIntake = new JoystickButton(manipulator, getButton("Close Intake @@ -107,5 +103,10 @@ public OI() { // intake.whenPressed(new IntakeCube()); // outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); // outake.whenPressed(new OutakeCube()); + + /*MoveLiftUpButton = new JoystickButton(manipulator, getButton("Run Lift Motor Up", 7)); + MoveLiftDownButton = new JoystickButton(manipulator, getButton("Run Lift Motor Down", 8)); + MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); + MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));*/ } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index a627bdc..c88fd70 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -36,12 +36,15 @@ public class RobotMap { public static PowerDistributionPanel pdp; - public static WPI_TalonSRX liftMotor; + public static WPI_VictorSPX liftMotorA; + public static WPI_TalonSRX liftMotorB; + public static WPI_TalonSRX liftMotorC; + public static SpeedControllerGroup liftMotors; public static Encoder liftEnc; 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; @@ -118,15 +121,18 @@ private void configSPX(WPI_VictorSPX mc) { public RobotMap() { pdp = new PowerDistributionPanel(); - - liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 7)); - configSRX(liftMotor); + + 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); liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4)); liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5)); liftEnc = new Encoder(liftEncPort1, liftEncPort2); liftEnc.setPIDSourceType(PIDSourceType.kDisplacement); - climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6)); - configSRX(climberMotor); leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9)); rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8)); 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 73f1d28..764551b 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java @@ -14,55 +14,40 @@ */ public class AutoLift extends Command implements PIDOutput{ /** - * All distances are - 1 foot for initial height of intake and + 3 inches for wiggle room for dropping cubes - * Also, acutal distances are divided by 3 because according to cad, the lift will have a 1:3 ratio from winch + * 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) - * 9.75 / 3 for ratio = 3.25 + * 21.75 / 3 for ratio = 7.25 */ - private final double SWITCH_DIST = 3.25; + private final double SWITCH_DIST = 7.25; /** * Distance to scale * 5 feet starting - * 51 / 3 = 17 + * 63 / 3 = 21 */ - private final double SCALE_DIST = 17; + private final double SCALE_DIST = 21; /** * Distance to bar - * 72 / 3 = 24 + * 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 = 24; + private final double BAR_DIST = 29; private double desiredDist = 0; - private double currDist = 0; + private double currDist; private LiftInterface lift; private Position desiredPos; private PIDController liftController; public AutoLift(Position stage, LiftInterface lift) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); this.lift = lift; requires(Robot.lift); - switch(lift.getCurrPos()) { - case GROUND: - currDist = 0; - break; - case SWITCH: - currDist = SWITCH_DIST; - break; - case SCALE: - currDist = SCALE_DIST; - break; - case BAR: - currDist = BAR_DIST; - break; - } + currDist = lift.getHeight(); switch(stage) { case GROUND: desiredDist = -currDist; @@ -86,7 +71,6 @@ public AutoLift(Position stage, LiftInterface lift) { // Called just before this Command runs the first time protected void initialize() { - lift.resetEnc(); // 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] @@ -104,7 +88,7 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { if(liftController.onTarget()) { - lift.setTargetPosition(desiredPos); + lift.setCurrPosition(desiredPos); return true; } return false; diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java index cf24d63..7cb94ee 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java @@ -11,7 +11,7 @@ */ public class Climber extends Subsystem implements ClimberInterface { - private final WPI_TalonSRX climberMotor = RobotMap.climberMotor; + //private final WPI_TalonSRX climberMotor = RobotMap.climberMotor; /** @@ -52,7 +52,7 @@ public void attachToBar() { * stops the climber */ public void stopClimber() { - climberMotor.stopMotor(); + //climberMotor.stopMotor(); } 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 215d838..62f62b5 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java @@ -1,11 +1,15 @@ package org.usfirst.frc.team199.Robot2018.subsystems; +import org.usfirst.frc.team199.Robot2018.OI; import org.usfirst.frc.team199.Robot2018.RobotMap; +import org.usfirst.frc.team199.Robot2018.commands.AutoLift; import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; /** @@ -13,9 +17,9 @@ */ public class Lift extends Subsystem implements LiftInterface { - private final WPI_TalonSRX liftMotor = RobotMap.liftMotor; + private final SpeedControllerGroup liftMotors = RobotMap.liftMotors; private final Encoder liftEnc = RobotMap.liftEnc; - private Position targetPosition = Position.GROUND; + private Position currPosition = Position.GROUND; /** * Set the default command for a subsystem here. @@ -23,47 +27,55 @@ public class Lift extends Subsystem implements LiftInterface { public void initDefaultCommand() { // Set the default command for a subsystem here. //setDefaultCommand(new MySpecialCommand()); + int ang = OI.manipulator.getPOV(); + Command com = null; + if(ang == 0) + com = new AutoLift(Position.GROUND, this); + else if(ang == 90) + com = new AutoLift(Position.SWITCH, this); + else if(ang == 180) + com = new AutoLift(Position.SWITCH, this); + else if(ang == 270) + com = new AutoLift(Position.SWITCH, this); + if(com != null) + setDefaultCommand(com); } - public void setTargetPosition(Position newPosition) { - targetPosition = newPosition; + /** + * Sets the current position in the lift subsystem + * @param newPosition - the new position meant to be set + */ + public void setCurrPosition(Position newPosition) { + currPosition = newPosition; } /** * Uses (insert sensor here) to detect the current lift position */ public double getHeight() { - return -1; + return liftEnc.getDistance() * 3; } /** * stops the lift */ public void stopLift() { - liftMotor.stopMotor(); + liftMotors.stopMotor(); } /** * gets current motor values */ public double getLiftSpeed() { - return liftMotor.get(); + return liftMotors.get(); } /** - * Goes to specified height - * @param position - ground, switch, scale, bar - * @param offset - distance up or down from the position - */ - public void goToPosition(Position position, double offset) { - - } - /** - * Runs lift motor at specified speed + * Runs lift motors at specified speed * @param speed - desired speed to run at */ public void runMotor(double output) { - liftMotor.set(output); + liftMotors.set(output); } /** @@ -71,7 +83,7 @@ public void runMotor(double output) { * @return pos - current position */ public Position getCurrPos() { - return targetPosition; + return currPosition; } /** * Resets the encoder 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 d07f38a..8ba11d4 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java @@ -31,16 +31,8 @@ public enum Position { */ public double getLiftSpeed(); - - /** - * Goes to specified height - * @param position - ground, switch, scale, bar - * @param offset - distance up or down from position - */ - public void goToPosition(Position position, double offset); - /** - * Runs lift motor at specified speed + * Runs lift motors at specified speed * @param speed - desired speed to run at */ public void runMotor(double speed); @@ -60,5 +52,7 @@ public enum Position { * Sets the current position in the lift subsystem * @param newPosition - the new position meant to be set */ - public void setTargetPosition(Position newPosition); + public void setCurrPosition(Position newPosition); + + } diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/LiftTester.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/LiftTester.java deleted file mode 100644 index e69de29..0000000 From 74059091e0a3d0a6a3c44d09391af7644bd761d3 Mon Sep 17 00:00:00 2001 From: Gabe Mitnick Date: Mon, 19 Feb 2018 13:56:17 -0800 Subject: [PATCH 5/7] add joystick functionalities to controllers.txt --- docs/controllers.txt | 119 +++++++++++++++++++++---------------------- 1 file changed, 59 insertions(+), 60 deletions(-) diff --git a/docs/controllers.txt b/docs/controllers.txt index 44ed967..ff922d3 100644 --- a/docs/controllers.txt +++ b/docs/controllers.txt @@ -1,60 +1,59 @@ -controller indices - left joystick [0] - buttons - 1 - 2 toggle drive type (arcade/tank) [ShiftDriveType] - 3 - 4 - 5 - 6 - 7 move with PID by inches from SmartDashboard "Move Targ" [PIDMove] - 8 turn with PID by degrees from SmartDashboard "Turn Targ" [PIDTurn] - 9 - 10 reset left and right drivetrain encoders to 0 [ResetEncoders] - 11 - axes - 0 X axis - 1 Y axis (forward is negative) - 2 Z axis (knob at base) - right joystick [1] - buttons - 1 - 2 shift drivetrain to low gear [ShiftLowGear] - 3 shift drivetrain to high gear [ShiftHighGear] - 4 - 5 - 6 - 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 run lift motor up [RunLift] - 11 run lift motor down [RunLift] - axes - 0 X axis - 1 Y axis (forward is negative) - 2 Z axis (knob at base) - manipulator [3] - buttons - 1 A close intake [CloseIntake] - 2 B open intake [OpenIntake] - 3 X raise intake [RaiseIntake] - 4 Y lower intake [LowerIntake] - 5 LB take in [IntakeCube] - 6 RB take out [OuttakeCube] - 7 back - 8 start - 9 N/a - 10 N/a - press mode to toggle mode light - axes - 0 left thumbstick X axis (D-pad if mode light on) - 1 left thumbstick Y axis (D-pad if mode light on) - 2 left trigger - 3 right trigger - 4 right thumbstick X axis - 5 right thumbstick Y axis - mode light off: - POV - D-pad (left joystick if mode light on) - rumble - fake news +left joystick [0] + buttons + 1 + 2 toggle drive type (arcade/tank) [ShiftDriveType] + 3 + 4 + 5 + 6 + 7 move with PID by inches from SmartDashboard "Move Targ" [PIDMove] + 8 turn with PID by degrees from SmartDashboard "Turn Targ" [PIDTurn] + 9 + 10 reset left and right drivetrain encoders to 0 [ResetEncoders] + 11 + axes + 0 X axis if NOT default: arcade turn + 1 Y axis tank left, arcade forward (if default) + 2 Z axis +right joystick [1] + buttons + 1 + 2 shift drivetrain to low gear [ShiftLowGear] + 3 shift drivetrain to high gear [ShiftHighGear] + 4 + 5 + 6 + 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 run lift motor up [RunLift] + 11 run lift motor down [RunLift] + axes + 0 X axis arcade turn (if default) + 1 Y axis tank right, if NOT default: arcade forward + 2 Z axis +manipulator [3] + buttons + 1 A close intake [CloseIntake] + 2 B open intake [OpenIntake] + 3 X raise intake [RaiseIntake] + 4 Y lower intake [LowerIntake] + 5 LB take in [IntakeCube] + 6 RB take out [OuttakeCube] + 7 back + 8 start + 9 N/a + 10 N/a + press mode to toggle mode light + axes + 0 left thumbstick X axis (D-pad if mode light on) + 1 left thumbstick Y axis (D-pad if mode light on) + 2 left trigger + 3 right trigger + 4 right thumbstick X axis + 5 right thumbstick Y axis + mode light off: + POV + D-pad (left joystick if mode light on) + rumble + fake news From 9f302350b17a155bbeffd634b0cbaad3b43d2530 Mon Sep 17 00:00:00 2001 From: nelsonychs Date: Mon, 19 Feb 2018 14:46:05 -0800 Subject: [PATCH 6/7] Accidentally didn't commit update doc --- docs/controllers.txt | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/docs/controllers.txt b/docs/controllers.txt index ff922d3..41e8dfe 100644 --- a/docs/controllers.txt +++ b/docs/controllers.txt @@ -26,8 +26,8 @@ right joystick [1] 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 run lift motor up [RunLift] - 11 run lift motor down [RunLift] + 10 + 11 axes 0 X axis arcade turn (if default) 1 Y axis tank right, if NOT default: arcade forward @@ -40,8 +40,8 @@ manipulator [3] 4 Y lower intake [LowerIntake] 5 LB take in [IntakeCube] 6 RB take out [OuttakeCube] - 7 back - 8 start + 7 back run lift motor up [RunLift] + 8 start run lift motor down [RunLift] 9 N/a 10 N/a press mode to toggle mode light @@ -55,5 +55,9 @@ manipulator [3] mode light off: POV D-pad (left joystick if mode light on) + UP move lift ground [AutoLift(GROUND)] + RIGHT move lift switch [AutoLift(SWITCH)] + DOWN move lift scale [AutoLift(SCALE)] + LEFT move lift bar [AutoLift(BAR)] rumble fake news From 8ee9a13751e24ba614aeb30de8f3b2f4b7e87d51 Mon Sep 17 00:00:00 2001 From: nelsonychs Date: Wed, 21 Feb 2018 15:16:40 -0800 Subject: [PATCH 7/7] PID Simulator for lift, currently tuning constants Couple of issues: Validity of constants that affect acceleration as well as PID constant tuning. On a separate note, all the commented lines with parameters on top of variables is for dynamic value testing ( part of junit ) . --- .../frc/team199/Robot2018/PIDConstSim.java | 275 ++++++++++++++++++ 1 file changed, 275 insertions(+) create mode 100644 Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java new file mode 100644 index 0000000..6359c69 --- /dev/null +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDConstSim.java @@ -0,0 +1,275 @@ +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 edu.wpi.first.wpilibj.HLUsageReporting; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +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 = ) + 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 + static final double kStallCurrent = 131; + // Free Speed in RPM + static final double kFreeSpeed = 5330; + // Free Current in Amps + static final double kFreeCurrent = 2.7; + // Mass of the Elevator + // 51.3 lbs + static final double kMass = 23.2693; + // Number of motors + static final double kNumMotors = 3.0; + // 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)); + // Torque constant + static final double Kt = (kNumMotors * kStallTorque) / kStallCurrent; + // Gear ratio + // 11.754 lift gear ratio reduction? + static final double kG = 11.754; + // Radius of pulley + static final double kr = 0.02273; + + // Control loop time step + static final double kDt = 0.01; + + double current_time = 0; + + // 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; + } + */ + + private double GetAcceleration(double 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; + } + + @BeforeEach + void setUp() { + // Since VelocityPIDController extends PIDController and PIDController calls + // static methods in wpilib that only work on robot, + // we setup these mocks to allow the tests to run off robot. + // can't start at 0 because it will crash timer + current_time = kDt; + 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; + } + }); + + Timer.SetImplementation(tim); + HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class); + HLUsageReporting.SetImplementation(usageReporter); + } + + class PIDSim{ + public PIDSim() { + //position = 0; + } + + public double pidGet() { + return position; + } + + public void pidWrite(double output) { + //System.out.println("voltage: " + output); + simulateTime(kDt, output * 12); + System.out.println("position: " + position); + } + } + + class PIDSimController extends PIDController{ + + public PIDSimController(double p, double i, double d, PIDSource src, PIDOutput out) { + super(p,i,d,src,out); + } + + public void calc() { + this.calculate(); + } + + } + + class PIDSimSource implements PIDSource{ + private PIDSim sim; + private PIDSourceType type; + public PIDSimSource(PIDSim sim) { + this.sim = sim; + } + + @Override + public PIDSourceType getPIDSourceType() { + return type; + } + + @Override + public double pidGet() { + return sim.pidGet(); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + // TODO Auto-generated method stub + type = pidSource; + } + } + + class PIDSimOutput implements PIDOutput{ + private PIDSim sim; + public PIDSimOutput(PIDSim sim) { + this.sim = sim; + } + @Override + public void pidWrite(double output) { + sim.pidWrite(output); + } + } + + void test24() { + PIDSim sim = new PIDSim(); + PIDSimSource src = new PIDSimSource(sim); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(0.6); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 100; i++) { + liftController.calc(); + } + System.out.println("Test" + targetOne + ": " + sim.pidGet()); + System.out.println("curr_time_1: " + current_time); + assertTrue(sim.pidGet() > 0); + assertTrue(Math.abs(0.6 - sim.pidGet()) < tolerance); + } + + void test60() { + PIDSim sim = new PIDSim(); + PIDSimSource src = new PIDSimSource(sim); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(1.524); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 200; i++) { + liftController.calc(); + } + System.out.println("" + sim.pidGet()); + assertTrue(sim.pidGet() > 0); + assertTrue(Math.abs(1.524 - sim.pidGet()) < tolerance); + } + + @Test + void test45() { + PIDSim sim = new PIDSim(); + PIDSimSource src = new PIDSimSource(sim); + src.setPIDSourceType(PIDSourceType.kDisplacement); + PIDSimOutput out = new PIDSimOutput(sim); + PIDSimController liftController = new PIDSimController(liftkP, liftkI, liftkD, src, out); + liftController.setSetpoint(1.143); + liftController.setOutputRange(-1.0, 1.0); + liftController.enable(); + //1 second? + for(int i = 0; i < 100; i++) { + liftController.calc(); + } + System.out.println("" + sim.pidGet()); + assertTrue(sim.pidGet() > 0); + assertTrue(Math.abs(1.143 - sim.pidGet()) < tolerance); + } + +}