diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 7e4e616..9834653 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -7,6 +7,11 @@ package org.usfirst.frc.team199.Robot2018; +import org.usfirst.frc.team199.Robot2018.commands.CloseIntake; +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.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; @@ -35,21 +40,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; + public static Joystick manipulator; + 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)) { @@ -65,13 +70,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()); @@ -85,27 +90,24 @@ 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 - // 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("Outtake Button", 6)); + // outtakeCubeButton.whenPressed(new OuttakeCube()); + + /*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/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 { 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/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); + } + +} 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 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); + } + +} diff --git a/docs/controllers.txt b/docs/controllers.txt index 6cd1c8c..41e8dfe 100644 --- a/docs/controllers.txt +++ b/docs/controllers.txt @@ -1,60 +1,63 @@ -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 - buttons - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - axes - 0 X axis - 1 Y axis (forward is negative) - 2 Z axis (knob at base) - right joystick - buttons - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - axes - 0 X axis - 1 Y axis (forward is negative) - 2 Z axis (knob at base) +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 + 11 + 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 run lift motor up [RunLift] + 8 start run lift motor down [RunLift] + 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) + 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