diff --git a/build.gradle b/build.gradle index fe51837..5d0d2da 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.4.1" } def ROBOT_MAIN_CLASS = "frc.robot.Main" diff --git a/src/main/java/frc/robot/DriveTrain.java b/src/main/java/frc/robot/DriveTrain.java index 52bffc3..f807813 100644 --- a/src/main/java/frc/robot/DriveTrain.java +++ b/src/main/java/frc/robot/DriveTrain.java @@ -22,7 +22,7 @@ public class DriveTrain { private final int rightPort1; private final int leftPort2; private final int rightPort2; - public final ADIS16448_IMU gyro = new ADIS16448_IMU(); + //public final ADIS16448_IMU gyro = new ADIS16448_IMU(); private int counter = 0; @@ -42,7 +42,7 @@ public DriveTrain(final int leftPort1, final int leftPort2, final int rightPort1 rightMotor2.setNeutralMode(NeutralMode.Brake); leftMotor1.setNeutralMode(NeutralMode.Brake); leftMotor2.setNeutralMode(NeutralMode.Brake); - gyro.reset(); + // gyro.reset(); //gyroPortNumber should be analong 0 or 1 } @@ -80,38 +80,12 @@ public void updateSpeed(final ThrottlePosition throttlePosition) { rightMotor1.set(ControlMode.PercentOutput, right); rightMotor2.follow(rightMotor1); - counter++; - if(counter % 10 == 0){ - double left1Current = leftMotor1.getOutputCurrent(); - double left1Voltage = leftMotor1.getMotorOutputVoltage(); - double right1Current = rightMotor1.getOutputCurrent(); - double right1Voltage = rightMotor1.getMotorOutputVoltage(); - double left2Voltage = leftMotor2.getMotorOutputVoltage(); - double right2Voltage = rightMotor2.getMotorOutputVoltage(); - System.out.print(String.format("Currents: R: %.2f L: %.2f ", right1Current, left1Current)); - System.out.print(String.format("Voltage: L1: %.2f R1: %.2f L2: %.2f R2: %.2f\n", left1Voltage, right1Voltage, left2Voltage, right2Voltage)); - - } - //makeVictorsFollowers(); } public void autoUpdateSpeed(double left, double right) { leftMotor1.set(ControlMode.PercentOutput, left); rightMotor1.set(ControlMode.PercentOutput, right); leftMotor2.follow(leftMotor1); rightMotor2.follow(rightMotor1); - - counter++; - if(counter % 10 == 0){ - double left1Current = leftMotor1.getOutputCurrent(); - double left1Voltage = leftMotor1.getMotorOutputVoltage(); - double right1Current = rightMotor1.getOutputCurrent(); - double right1Voltage = rightMotor1.getMotorOutputVoltage(); - double left2Voltage = leftMotor2.getMotorOutputVoltage(); - double right2Voltage = rightMotor2.getMotorOutputVoltage(); - System.out.print(String.format("Currents: R: %.2f L: %.2f ", right1Current, left1Current)); - System.out.print(String.format("Voltage: L1: %.2f R1: %.2f L2: %.2f R2: %.2f\n", left1Voltage, right1Voltage, left2Voltage, right2Voltage)); - - } } public TalonSRX getLeftMotor() { return leftMotor1; @@ -119,9 +93,9 @@ public TalonSRX getLeftMotor() { public TalonSRX getRightMotor() { return rightMotor1; } - public ADIS16448_IMU getGyro() { - return gyro; - } + //public ADIS16448_IMU getGyro() { + // return gyro; + //} public void getEncoderPosition(){ int encoderPositionLeft = leftMotor1.getSelectedSensorPosition(); diff --git a/src/main/java/frc/robot/Elevator.java b/src/main/java/frc/robot/Elevator.java index 6cc4491..e3c7a08 100644 --- a/src/main/java/frc/robot/Elevator.java +++ b/src/main/java/frc/robot/Elevator.java @@ -36,6 +36,8 @@ public Elevator(int elevatorPort , int limitSwitchPort){ talon = new TalonSRX(elevatorPort); talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); talon.setSensorPhase(true); + talon.configMotionCruiseVelocity(1100); + talon.configMotionAcceleration(1100); talon.setSelectedSensorPosition(0); // talon.config_kD(0, dConstant); // talon.config_kP(0, pConstant); @@ -58,9 +60,9 @@ public void configurePID() { //@param distance is in inches public void move(double distance){ talon.setIntegralAccumulator(0); - int talonPosition = talon.getSelectedSensorPosition(); + //int talonPosition = talon.getSelectedSensorPosition(); double totalPulses = (distance/(diameter*Math.PI)) * pulsesPerRevolution; - talon.set(ControlMode.Position, totalPulses); + talon.set(ControlMode.MotionMagic, totalPulses); double pulsesAfter = talon.getSelectedSensorPosition(); } diff --git a/src/main/java/frc/robot/Grabber.java b/src/main/java/frc/robot/Grabber.java index b0a1bb0..d316922 100644 --- a/src/main/java/frc/robot/Grabber.java +++ b/src/main/java/frc/robot/Grabber.java @@ -9,10 +9,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Grabber { + //this worked private VictorSP leftFlywheel; private VictorSP rightFlywheel; private TalonSRX rightClaw; - private TalonSRX leftClaw; + public TalonSRX leftClaw; private DigitalInput leftLimitSwitch; private DigitalInput rightLimitSwitch; private final double flywheelSpeed = 0.5; @@ -25,6 +26,7 @@ public class Grabber { private final double openRevolutions = 12.5; private final double cargoRevolutions = 8.33; private final double hatchRevolutions = 1.389; + private final double testRevolution = 1.389*2; private final double closedRevolutions = 0; @@ -40,28 +42,42 @@ public Grabber(int leftFlywheelPort , int rightFlywheelPort , int leftClawPort , SmartDashboard.putNumber("pid/claw/d", 0.0); SmartDashboard.putNumber("pid/claw/f", 0.0); - //rightClaw.config_kD(0, dConstant); - //rightClaw.config_kP(0, pConstant); - //rightClaw.config_kI(0, iConstant); - // rightClaw.config_kF(0, fConstant); rightClaw.configPeakCurrentLimit(maxAmps); rightClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + rightClaw.setInverted(true); + rightClaw.setSensorPhase(true); - //leftClaw.config_kD(0, dConstant); - //leftClaw.config_kP(0, pConstant); - //leftClaw.config_kI(0, iConstant); - //leftClaw.config_kF(0, fConstant); leftClaw.configPeakCurrentLimit(maxAmps); leftClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + leftClaw.setSensorPhase(true); + leftClaw.configMotionCruiseVelocity(3000); + leftClaw.configMotionAcceleration(6000); + + rightClaw.configMotionCruiseVelocity(3000); + rightClaw.configMotionAcceleration(6000); + + + rightClaw.setSelectedSensorPosition(0); + leftClaw.setSelectedSensorPosition(0); leftLimitSwitch = new DigitalInput(leftLimitPort); rightLimitSwitch = new DigitalInput(rightLimitPort); } + + public void initializeSettings(){ + rightClaw.configPeakCurrentLimit(maxAmps); + rightClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + leftClaw.setSensorPhase(true); + leftClaw.configPeakCurrentLimit(maxAmps); + leftClaw.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); + + } public void configurePID() { this.pConstant = SmartDashboard.getNumber("pid/claw/p", 0.2); this.iConstant = SmartDashboard.getNumber("pid/claw/i", pConstant / 10000); this.dConstant = SmartDashboard.getNumber("pid/claw/d", 0.0); this.fConstant = SmartDashboard.getNumber("pid/claw/f", 0.0); + this.iConstant = (this.iConstant*this.pConstant)/1000; leftClaw.config_kD(0, dConstant); leftClaw.config_kP(0, pConstant); leftClaw.config_kI(0, iConstant); @@ -71,25 +87,72 @@ public void configurePID() { rightClaw.config_kP(0, pConstant); rightClaw.config_kI(0, iConstant); rightClaw.config_kF(0, fConstant); + } public void open(){ - move(openRevolutions); + try { + move(openRevolutions); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } public void cargo(){ - move(cargoRevolutions); + try { + move(cargoRevolutions); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } public void hatch(){ - move(hatchRevolutions); + try { + move(hatchRevolutions); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } public void closed(){ - move(closedRevolutions); + try { + move(closedRevolutions); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } - public void move(double numRevolutions){ - double totalPulses = numRevolutions * pulsesPerRevolution; - leftClaw.set(ControlMode.Position, totalPulses); - rightClaw.set(ControlMode.Position , totalPulses); + public void move(double numRevolutions) throws InterruptedException { + leftClaw.setIntegralAccumulator(0); + rightClaw.setIntegralAccumulator(0); + leftClaw.setSelectedSensorPosition(0); + rightClaw.setSelectedSensorPosition(0); + double totalPulses = numRevolutions * pulsesPerRevolution; + System.out.println("About to move"); + //leftClaw.set(ControlMode.MotionMagic, totalPulses); + rightClaw.set(ControlMode.MotionMagic , totalPulses); + System.out.println("Move complete"); + + double motorVoltage = leftClaw.getMotorOutputVoltage(); + + // double pulsesAfter = rightClaw.getSelectedSensorPosition(); + //int closedLoopError = rightClaw.getClosedLoopError(); + //double motorOutput = rightClaw.getMotorOutputPercent(); + double leftPulsesAfter = leftClaw.getSelectedSensorPosition(); + double leftClosedLoopError = leftClaw.getClosedLoopError(); + double leftMotorVoltage = leftClaw.getMotorOutputVoltage(); + // System.out.println("Right Voltage: " + motorVoltage + "Position after: " + pulsesAfter + "Closed Loop Error" + closedLoopError); + System.out.println("Left Voltage: " + leftMotorVoltage + "Position after: " + leftPulsesAfter + "Closed Loop Error" + leftClosedLoopError); + } + + public void testTenDegrees(){ + try { + move(testRevolution); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } public void toLimitSwitches(){ @@ -110,25 +173,41 @@ public void toLimitSwitches(){ } - public void intake(){ + public void output(){ leftFlywheel.set(flywheelSpeed); rightFlywheel.set(-flywheelSpeed); } - public void output(){ + public void intake(){ leftFlywheel.set(-flywheelSpeed); rightFlywheel.set(flywheelSpeed); } + public void zeroPosition(){ + leftClaw.setSelectedSensorPosition(0); + rightClaw.setSelectedSensorPosition(0); + } + public void stop(){ leftFlywheel.set(0); rightFlywheel.set(0); } - public void resetToFactorySettings(){ - rightClaw.configFactoryDefault(); - leftClaw.configFactoryDefault(); + public void testEncoderPosition(){ + double rightBeginningPosition = rightClaw.getSelectedSensorPosition(); + double leftBeginningPosition = leftClaw.getSelectedSensorPosition(); + System.out.println("Left: " + leftBeginningPosition + " Right: " + rightBeginningPosition); + //leftClaw.set(ControlMode.PercentOutput , 0.2); + double leftAfter = leftClaw.getSelectedSensorPosition(); + double rightAfter = rightClaw.getSelectedSensorPosition(); + System.out.println("Left: " + leftAfter + " Right: " + rightAfter); + + } + + public void spinClawMotor(){ + leftClaw.set(ControlMode.PercentOutput , 0.4); + //rightClaw.set(ControlMode.PercentOutput , 0.4); } diff --git a/src/main/java/frc/robot/MotionProfiling.java b/src/main/java/frc/robot/MotionProfiling.java index 368145e..fedcfdb 100644 --- a/src/main/java/frc/robot/MotionProfiling.java +++ b/src/main/java/frc/robot/MotionProfiling.java @@ -45,13 +45,13 @@ public MotionProfiling(DriveTrain driveTrain, String setupLeft , String setupRig public void update() { double l = left.calculate(leftMotor.getSelectedSensorPosition()); double r = right.calculate(rightMotor.getSelectedSensorPosition()); - double gyroHeading = driveTrain.getGyro().getAngleY(); // Assuming the gyro is giving a value in degrees - double desiredHeading = -Pathfinder.r2d(left.getHeading()); // Should also be in degrees + //double gyroHeading = driveTrain.getGyro().getAngleY(); // Assuming the gyro is giving a value in degrees + //double desiredHeading = -Pathfinder.r2d(left.getHeading()); // Should also be in degrees - double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading); - double turn = 0.8 * (-1.0/80.0) * angleDifference; + //double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading); + //double turn = 0.8 * (-1.0/80.0) * angleDifference; - driveTrain.autoUpdateSpeed(l + turn, r - turn); + //driveTrain.autoUpdateSpeed(l + turn, r - turn); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5a9e9e6..caef387 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -51,16 +51,20 @@ public void robotInit() { elevator.configurePID(); grabber.configurePID(); arm.configurePID(); - elevator.resetToFactorySettings(); - grabber.resetToFactorySettings(); + //elevator.resetToFactorySettings(); + //grabber.resetToFactorySettings(); driveTrain.autoUpdateSpeed(0,0); for (int i = 0; i < pathFiles.length; i++) { pathFiles[i] = String.format(filePath , i + 1); } - joystick1.addButton(2, elevator::lowCargo); - joystick1.addButton(3, elevator::lowHatch); - joystick1.addButton(4, elevator::mediumHigh); - joystick1.addButton(5, elevator::zeroLimitSwitch); + joystick1.addButton(2, grabber::hatch); + joystick1.addButton(3, grabber::cargo); + joystick1.addButton(4, grabber::open); + //joystick1.addButton(8 , grabber::intake , grabber::stop); + //joystick1.addButton(9 , grabber::output , grabber:: stop); + joystick1.addButton(8 , elevator::lowCargo); + joystick1.addButton(9 , elevator::lowHatch); + } /** @@ -117,9 +121,13 @@ public void teleopInit(){ elevator.configurePID(); grabber.configurePID(); arm.configurePID(); - elevator.resetToFactorySettings(); - grabber.resetToFactorySettings(); + grabber.zeroPosition(); + //elevator.resetToFactorySettings(); + //grabber.resetToFactorySettings(); + + //grabber.initializeSettings(); driveTrain.autoUpdateSpeed(0,0); + } @@ -156,6 +164,9 @@ public void teleopPeriodic() { else{ joystick1.listen(); } + System.out.println("Voltage: " + grabber.leftClaw.getMotorOutputVoltage() + "Error: " + grabber.leftClaw.getClosedLoopError() + "Current:" + grabber.leftClaw.getOutputCurrent()); + + } public void changeDriverControl(){ @@ -174,5 +185,7 @@ private void updateSmartDB(){ */ @Override public void testPeriodic() { + grabber.testEncoderPosition(); + } }