From 9307a2dd5842de4a51deff4bf894a296752ea3f5 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 14 Jan 2023 12:42:16 -0800 Subject: [PATCH 01/30] copied over arm file and can id --- gradlew | 0 src/main/java/frc/robot/Constants.java | 2 + .../frc/robot/subsystems/ArmSubsystem.java | 38 +++++++++++++++++++ 3 files changed, 40 insertions(+) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/subsystems/ArmSubsystem.java diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a06c1a7..e00b5ce 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,4 +57,6 @@ public final class Constants { public static final double WHEEL_DIAMETER= 4; //inches public static final double TICKS_PER_REV = 2048; public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder + + public static final int ARM_MOTOR_ID = 7; } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java new file mode 100644 index 0000000..e6a5090 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import frc.robot.Constants; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.math.controller.PIDController; + +public class ArmSubsystem { + + //variables go here + double armLengthKP = 0.04; //actual vals to be determined + double armLengthKI = 0.000002; + double armLengthKD = 0.005; + PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + + TalonFX armMotor = new TalonFX(Constants.ARM_MOTOR_ID); + + public ArmSubsystem(){ + + } + + public double getArmLength(){ + return armMotor.getSelectedSensorPosition(); + } + + public void armPID(double armLengthSetpoint){ + armLengthController.setSetpoint(armLengthSetpoint); + double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); + System.out.println("output: " + output); + armMotor.set(ControlMode.PercentOutput, output); + } + + public void telescoping(double armSetpoint){//setpoint in inches + armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant + armMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm + } + +} From 064579d796ecddf7410fc05db2e830891c083d6b Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Tue, 17 Jan 2023 19:19:21 -0800 Subject: [PATCH 02/30] telescopingarm sensor, gain, deadband and buttons --- src/main/java/frc/robot/OI.java | 3 +++ src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/ArmSubsystem.java | 19 +++++++++++++------ 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index ea7a14f..245a530 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -7,4 +7,7 @@ public class OI { public static final XboxController m_controller = new XboxController(0); public static Button stop = Button.kB; + public static Button armExtended = Button.kY;//button to fully extend to 90 degrees + public static Button swingThrough = Button.kX;//button to go through + public static Button armControl = Button.kLeftStick;//stick to control arm length (any length from fully retracted to extended) } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b2d65bc..75ff7bc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -138,4 +138,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index e6a5090..5afe1f5 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -2,7 +2,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import frc.robot.Constants; -import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.ControlMode;//written as import com.ctre.phoenix.motorcontrol.TalonFXControlMode; in some documentation. is ours okay?? import edu.wpi.first.math.controller.PIDController; public class ArmSubsystem { @@ -15,6 +15,8 @@ public class ArmSubsystem { TalonFX armMotor = new TalonFX(Constants.ARM_MOTOR_ID); + armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + public ArmSubsystem(){ } @@ -23,13 +25,18 @@ public double getArmLength(){ return armMotor.getSelectedSensorPosition(); } - public void armPID(double armLengthSetpoint){ - armLengthController.setSetpoint(armLengthSetpoint); - double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); - System.out.println("output: " + output); - armMotor.set(ControlMode.PercentOutput, output); + if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { + /* Within 10% of zero */ + leftYstick = 0;//left joystick y axis in documentation. assign later } + //public void armPID(double armLengthSetpoint){ + // armLengthController.setSetpoint(armLengthSetpoint); + //double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); + //System.out.println("output: " + output); + //armMotor.set(ControlMode.PercentOutput, output);//written as _talon.set(TalonFXControlMode.PercentOutput, leftYstick); in documentation. leftystick is joy.gety + //} + public void telescoping(double armSetpoint){//setpoint in inches armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant armMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm From d97532f1d6d260330f9ffc298a51e87ead5072e0 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Thu, 19 Jan 2023 18:58:18 -0800 Subject: [PATCH 03/30] created telescoping arm state machine, init and periodic methods, implemented moveArm method --- .../frc/robot/subsystems/ArmSubsystem.java | 61 ++++++++++++++----- 1 file changed, 46 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 5afe1f5..6987da4 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -8,28 +8,59 @@ public class ArmSubsystem { //variables go here - double armLengthKP = 0.04; //actual vals to be determined - double armLengthKI = 0.000002; - double armLengthKD = 0.005; - PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + //double armLengthKP = 0.04; //actual vals to be determined + //double armLengthKI = 0.000002; + //double armLengthKD = 0.005; + //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + public static ArmStates armState = ArmStates.OFF; TalonFX armMotor = new TalonFX(Constants.ARM_MOTOR_ID); - armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us - public ArmSubsystem(){ + public static enum ArmStates{ + OFF, //fully retracted + FULLY_EXTENDED, + LOW_ARM_LENGTH, // arm length when scoring on low node + MID_ARM_LENGTH, // also shelf height + HIGH_ARM_LENGTH; + } + + public void setState(ArmStates newState){ + armState = newState; + } + public ArmSubsystem(){} + + public void init(){ + armMotor.setInverted(false); } - public double getArmLength(){ - return armMotor.getSelectedSensorPosition(); + public void periodic(){ + if (armState == ArmStates.OFF){ + armMotor.set(ControlMode.Position, 0); + armMotor.set(ControlMode.PercentOutput, 0); + } else if (armState == ArmStates.FULLY_EXTENDED){ + armMotor.set(ControlMode.Position, 56.25); //confirmed + } else if (armState == ArmStates.LOW_ARM_LENGTH){ + armMotor.set(ControlMode.Position, 8); // replace position value w low length + }else if (armState == ArmStates.MID_ARM_LENGTH){ + armMotor.set(ControlMode.Position, 5); // goes with 90 degrees rotation // replace position value w mid length + }else{ + armMotor.set(ControlMode.Position, 15); // replace position value w high length + } } - if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { - /* Within 10% of zero */ - leftYstick = 0;//left joystick y axis in documentation. assign later + + public void moveArm(double armSetpoint){//setpoint in inches + armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant + armMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm } + public double getArmLength(){ + return armMotor.getSelectedSensorPosition(); + } + //public void armPID(double armLengthSetpoint){ // armLengthController.setSetpoint(armLengthSetpoint); //double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); @@ -37,9 +68,9 @@ public double getArmLength(){ //armMotor.set(ControlMode.PercentOutput, output);//written as _talon.set(TalonFXControlMode.PercentOutput, leftYstick); in documentation. leftystick is joy.gety //} - public void telescoping(double armSetpoint){//setpoint in inches - armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant - armMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm - } + // if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { + // /* Within 10% of zero */ + // leftYstick = 0;//left joystick y axis in documentation. assign later + // } } From 14d9e87fda4ce051d791d02dd03ca250b2af085f Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Fri, 20 Jan 2023 19:19:42 -0800 Subject: [PATCH 04/30] added arm rotation and edited telescoping --- src/main/java/frc/robot/Constants.java | 4 +- .../subsystems/ArmRotationSubsystem.java | 72 ++++++++++++++++ .../frc/robot/subsystems/ArmSubsystem.java | 76 ----------------- .../subsystems/ArmTelescopingSubsystem.java | 85 +++++++++++++++++++ 4 files changed, 160 insertions(+), 77 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ArmSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e00b5ce..5789125 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,6 +57,8 @@ public final class Constants { public static final double WHEEL_DIAMETER= 4; //inches public static final double TICKS_PER_REV = 2048; public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder + public static final double ARM_ROTATION_GEAR_RATIO = 0.0;//this is undecided as of Jan 20th - public static final int ARM_MOTOR_ID = 7; + public static final int TELESCOPING_MOTOR_ID = 7; + public static final int ARM_ROTATION_MOTOR_ID = 8; } diff --git a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java new file mode 100644 index 0000000..ec7385b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java @@ -0,0 +1,72 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import frc.robot.Constants; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.math.controller.PIDController; +import com.ctre.phoenix.motorcontrol.NeutralMode; + + +public class ArmRotationSubsystem { + //variables go here + //double armLengthKP = 0.04; //actual vals to be determined + //double armLengthKI = 0.000002; + //double armLengthKD = 0.005; + //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + public static ArmRotationStates rState = ArmRotationStates.ZERO;//should this be retracted or mid? what is the equivalent to off? + + TalonFX armRotationMotor = new TalonFX(Constants.ARM_ROTATION_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive + + //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + + + public static enum ArmRotationStates{ + ZERO, //vertical + NINETY, + MID_ROTATION_ANGLE, //off the shelf + LOW_ROTATION_ANGLE, + HIGH_ROTATION_ANGLE; + } + + public void setRState(ArmRotationStates newState){ + rState = newState; + } + + public ArmRotationSubsystem(){} + + public void init(){ + armRotationMotor.setInverted(false); + armRotationMotor.setNeutralMode(NeutralMode.Brake); + //need to confirm with kim that we can zero motor when arm is vertical, then have negative 90 and 90 respectively when it rotates + } + + public void periodic(){ + if (rState == ArmRotationStates.ZERO){ + armRotationMotor.set(ControlMode.Position, 0); + armRotationMotor.set(ControlMode.PercentOutput, 0); + } else if (rState == ArmRotationStates.NINETY){ + armRotationMotor.set(ControlMode.Position, 90); //the following values are arbitrary + } else if (rState == ArmRotationStates.LOW_ROTATION_ANGLE){ + armRotationMotor.set(ControlMode.Position, 8); + }else if (rState == ArmRotationStates.HIGH_ROTATION_ANGLE){ + armRotationMotor.set(ControlMode.Position, 5); + }else{ + armRotationMotor.set(ControlMode.Position, 15); + } + } + + + public void rotateArm(double armRotationSetpoint){//setpoint in radians + armRotationSetpoint = armRotationSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant + armRotationMotor.set(ControlMode.Position, armRotationSetpoint);//change to whatever motor we use for arm + } + + public double ticksToDegrees(double ticks){ + return(ticks * 360 * Constants.ARM_ROTATION_GEAR_RATIO / Constants.TICKS_PER_REV); + } + + public double getRotationAngle(){ + return ticksToDegrees(armRotationMotor.getSelectedSensorPosition());//returns current rotation angle + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java deleted file mode 100644 index 6987da4..0000000 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import frc.robot.Constants; -import com.ctre.phoenix.motorcontrol.ControlMode;//written as import com.ctre.phoenix.motorcontrol.TalonFXControlMode; in some documentation. is ours okay?? -import edu.wpi.first.math.controller.PIDController; - -public class ArmSubsystem { - - //variables go here - //double armLengthKP = 0.04; //actual vals to be determined - //double armLengthKI = 0.000002; - //double armLengthKD = 0.005; - //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); - public static ArmStates armState = ArmStates.OFF; - - TalonFX armMotor = new TalonFX(Constants.ARM_MOTOR_ID); - - //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us - - public static enum ArmStates{ - OFF, //fully retracted - FULLY_EXTENDED, - LOW_ARM_LENGTH, // arm length when scoring on low node - MID_ARM_LENGTH, // also shelf height - HIGH_ARM_LENGTH; - } - - public void setState(ArmStates newState){ - armState = newState; - } - - public ArmSubsystem(){} - - public void init(){ - armMotor.setInverted(false); - } - - public void periodic(){ - if (armState == ArmStates.OFF){ - armMotor.set(ControlMode.Position, 0); - armMotor.set(ControlMode.PercentOutput, 0); - } else if (armState == ArmStates.FULLY_EXTENDED){ - armMotor.set(ControlMode.Position, 56.25); //confirmed - } else if (armState == ArmStates.LOW_ARM_LENGTH){ - armMotor.set(ControlMode.Position, 8); // replace position value w low length - }else if (armState == ArmStates.MID_ARM_LENGTH){ - armMotor.set(ControlMode.Position, 5); // goes with 90 degrees rotation // replace position value w mid length - }else{ - armMotor.set(ControlMode.Position, 15); // replace position value w high length - } - } - - - public void moveArm(double armSetpoint){//setpoint in inches - armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant - armMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm - } - - public double getArmLength(){ - return armMotor.getSelectedSensorPosition(); - } - - //public void armPID(double armLengthSetpoint){ - // armLengthController.setSetpoint(armLengthSetpoint); - //double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); - //System.out.println("output: " + output); - //armMotor.set(ControlMode.PercentOutput, output);//written as _talon.set(TalonFXControlMode.PercentOutput, leftYstick); in documentation. leftystick is joy.gety - //} - - // if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { - // /* Within 10% of zero */ - // leftYstick = 0;//left joystick y axis in documentation. assign later - // } - -} diff --git a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java new file mode 100644 index 0000000..1e37d79 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import frc.robot.Constants; +import com.ctre.phoenix.motorcontrol.ControlMode; + +public class ArmTelescopingSubsystem { + + //variables go here + //double armLengthKP = 0.04; //actual vals to be determined + //double armLengthKI = 0.000002; + //double armLengthKD = 0.005; + //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + public static TelescopingStates tState = TelescopingStates.RETRACTED;//should this be retracted or mid? what is the equivalent to off? + + TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive + + //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + + /*public static enum ArmStates{ + OFF, //fully retracted + FULLY_EXTENDED, + LOW_ARM_LENGTH, // arm length when scoring on low node + MID_ARM_LENGTH, // also shelf height + HIGH_ARM_LENGTH; + }*/ + + double extensionVal = 56.26;//this value should be the full extension length of the arm minus the length of it at zero + + public static enum TelescopingStates{ + RETRACTED, + FULLY_EXTENDED, + LOW_ARM_LENGTH, + MID_ARM_LENGTH, + HIGH_ARM_LENGTH; + } + + public void setTState(TelescopingStates newState){ + tState = newState; + } + + public ArmTelescopingSubsystem(){} + + public void init(){ + telescopingMotor.setInverted(false); + } + + public void periodic(){//sam requests that we can operate arm length by stick on xbox + if (tState == TelescopingStates.RETRACTED){ + telescopingMotor.set(ControlMode.Position, 0); + telescopingMotor.set(ControlMode.PercentOutput, 0); + } else if (tState == TelescopingStates.FULLY_EXTENDED){ + telescopingMotor.set(ControlMode.Position, extensionVal); //confirmed + } else if (tState == TelescopingStates.LOW_ARM_LENGTH){ + telescopingMotor.set(ControlMode.Position, 8); // replace position value w low length + }else if (tState == TelescopingStates.MID_ARM_LENGTH){ + telescopingMotor.set(ControlMode.Position, 5); // goes with 90 degrees rotation // replace position value w mid length + }else{ + telescopingMotor.set(ControlMode.Position, 15); // replace position value w high length + } + } + + + public void moveArm(double armSetpoint){//setpoint in inches + armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant + telescopingMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm + } + + public double getArmPosition(){ + return telescopingMotor.getSelectedSensorPosition(); + } + + //public void armPID(double armLengthSetpoint){ + // armLengthController.setSetpoint(armLengthSetpoint); + //double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); + //System.out.println("output: " + output); + //armMotor.set(ControlMode.PercentOutput, output);//written as _talon.set(TalonFXControlMode.PercentOutput, leftYstick); in documentation. leftystick is joy.gety + //} + + // if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { + // /* Within 10% of zero */ + // leftYstick = 0;//left joystick y axis in documentation. assign later + // } + +} From c3ece1408ce8a239eef6a506571ba9acd77d3a31 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Sat, 21 Jan 2023 10:38:50 -0800 Subject: [PATCH 05/30] skeleton for elevator subsystem --- .../robot/subsystems/ElevatorSubsystem.java | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..d9f4b43 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import frc.robot.Constants; + +public class ElevatorSubsystem { + + public ElevatorSubsystem(){} + + public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); + public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; + + public static enum ElevatorStates{ + ZERO, + LOW_ELEVATOR_HEIGHT, + MID_ELEVATOR_HEIGHT, + HIGH_ELEVATOR_HEIGHT; + } + + public void init(){ + elevatorMotor.setInverted(false); + elevatorMotor.setNeutralMode(NeutralMode.Brake); + } + + public void periodic(){ + if (elevatorState == ElevatorStates.ZERO){ + + } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ + + } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ + + } else { + + } + } + + public void setState(ElevatorStates newElevatorState){ + elevatorState = newElevatorState; + } +} From 374ea1979daafa5d7059f4820c3bd3869c7a7b10 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Sat, 21 Jan 2023 11:36:04 -0800 Subject: [PATCH 06/30] added gains, sensorPhase constant, and deadband constant --- src/main/java/frc/robot/Constants.java | 12 ++++++++++++ src/main/java/frc/robot/Gains.java | 21 +++++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 src/main/java/frc/robot/Gains.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5789125..028a52a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -61,4 +61,16 @@ public final class Constants { public static final int TELESCOPING_MOTOR_ID = 7; public static final int ARM_ROTATION_MOTOR_ID = 8; + public static final int ELEVATOR_CAN_ID = 9; + + /** + * Gains used in Positon Closed Loop, to be adjusted accordingly + * Gains(kp, ki, kd, kf, izone, peak output); + */ + static final Gains kGains = new Gains(0.15, 0.0, 1.0, 0, 1.0); + + /* Choose so that Talon does not report sensor out of phase */ + public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. + + public static double deadband = 0.5; //arbitrary } diff --git a/src/main/java/frc/robot/Gains.java b/src/main/java/frc/robot/Gains.java new file mode 100644 index 0000000..5ac7ce9 --- /dev/null +++ b/src/main/java/frc/robot/Gains.java @@ -0,0 +1,21 @@ +/** + * Class that organizes gains used when assigning values to slots + */ +package frc.robot; + +public class Gains { //Gain is a proportional value that shows the relationship between the magnitude of an input signal to the magnitude of an output signal at steady-state. + public final double kP; + public final double kI; + public final double kD; + public final int kIzone; //can be used to auto clear the integral accumulator if the sensor pos is too far from the target. This prevent unstable oscillation if the kI is too large. Value is in sensor units. + public final double kPeakOutput; //Absolute max motor output during closed-loop control modes only. A value of ‘1’ represents full output in both directions. + + + public Gains(double _kP, double _kI, double _kD, int _kIzone, double _kPeakOutput){ + kP = _kP; + kI = _kI; + kD = _kD; + kIzone = _kIzone; + kPeakOutput = _kPeakOutput; + } +} From 5acbcb966182a636b4e3fd9834e6083439ea83e9 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 21 Jan 2023 14:57:02 -0800 Subject: [PATCH 07/30] configured gains --- src/main/java/frc/robot/Constants.java | 3 ++ src/main/java/frc/robot/Gains.java | 4 ++- .../subsystems/ArmRotationSubsystem.java | 32 +++++++++++++++---- .../subsystems/ArmTelescopingSubsystem.java | 25 ++++++++++++--- .../robot/subsystems/ElevatorSubsystem.java | 32 ++++++++++++++++--- 5 files changed, 79 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 028a52a..19e70b5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,6 +63,9 @@ public final class Constants { public static final int ARM_ROTATION_MOTOR_ID = 8; public static final int ELEVATOR_CAN_ID = 9; + public static final int kTimeoutMs = 1000; + public static final int kPIDLoopIdx = 0;//not sure what this value does or if this value should be diff for rotation, telescoping, elevator + /** * Gains used in Positon Closed Loop, to be adjusted accordingly * Gains(kp, ki, kd, kf, izone, peak output); diff --git a/src/main/java/frc/robot/Gains.java b/src/main/java/frc/robot/Gains.java index 5ac7ce9..fcf204b 100644 --- a/src/main/java/frc/robot/Gains.java +++ b/src/main/java/frc/robot/Gains.java @@ -7,14 +7,16 @@ public class Gains { //Gain is a proportional value that shows the relationship public final double kP; public final double kI; public final double kD; + public final double kF; public final int kIzone; //can be used to auto clear the integral accumulator if the sensor pos is too far from the target. This prevent unstable oscillation if the kI is too large. Value is in sensor units. public final double kPeakOutput; //Absolute max motor output during closed-loop control modes only. A value of ‘1’ represents full output in both directions. - public Gains(double _kP, double _kI, double _kD, int _kIzone, double _kPeakOutput){ + public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput){ kP = _kP; kI = _kI; kD = _kD; + kF = _kF; kIzone = _kIzone; kPeakOutput = _kPeakOutput; } diff --git a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java index ec7385b..aaf11a7 100644 --- a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java @@ -2,22 +2,28 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import frc.robot.Constants; +import frc.robot.Gains;//i think this means we do not need to have our own gains file + import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.math.controller.PIDController; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController.*; public class ArmRotationSubsystem { - //variables go here - //double armLengthKP = 0.04; //actual vals to be determined - //double armLengthKI = 0.000002; - //double armLengthKD = 0.005; - //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + //these values are to be determined (untested) + double _kP = 1.0; + double _kI = 0.0; + double _kD = 0.0; + double _kF = 0.0; + int _kIzone = 0; + double _kPeakOutput = 0.0; + public static ArmRotationStates rState = ArmRotationStates.ZERO;//should this be retracted or mid? what is the equivalent to off? TalonFX armRotationMotor = new TalonFX(Constants.ARM_ROTATION_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive - //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + Gains armRotationGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); public static enum ArmRotationStates{ @@ -38,8 +44,17 @@ public void init(){ armRotationMotor.setInverted(false); armRotationMotor.setNeutralMode(NeutralMode.Brake); //need to confirm with kim that we can zero motor when arm is vertical, then have negative 90 and 90 respectively when it rotates + + //configuring deadband + armRotationMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ + armRotationMotor.config_kF(Constants.kPIDLoopIdx, armRotationGains.kF, Constants.kTimeoutMs); + armRotationMotor.config_kP(Constants.kPIDLoopIdx, armRotationGains.kP, Constants.kTimeoutMs); + armRotationMotor.config_kI(Constants.kPIDLoopIdx, armRotationGains.kI, Constants.kTimeoutMs); + armRotationMotor.config_kD(Constants.kPIDLoopIdx, armRotationGains.kD, Constants.kTimeoutMs); } + public void periodic(){ if (rState == ArmRotationStates.ZERO){ armRotationMotor.set(ControlMode.Position, 0); @@ -69,4 +84,9 @@ public double getRotationAngle(){ return ticksToDegrees(armRotationMotor.getSelectedSensorPosition());//returns current rotation angle } + //potentiometer? flight sensor? + //AD worried about slipping with encoder and gravity + //absolute tick count + //KBR says no worries + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java index 1e37d79..db3372c 100644 --- a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java @@ -2,19 +2,26 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import frc.robot.Constants; +import frc.robot.Gains; + import com.ctre.phoenix.motorcontrol.ControlMode; public class ArmTelescopingSubsystem { - //variables go here - //double armLengthKP = 0.04; //actual vals to be determined - //double armLengthKI = 0.000002; - //double armLengthKD = 0.005; - //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + //these values are to be determined (untested) + double _kP = 1.0; + double _kI = 0.0; + double _kD = 0.0; + double _kF = 0.0; + int _kIzone = 0; + double _kPeakOutput = 0.0; + public static TelescopingStates tState = TelescopingStates.RETRACTED;//should this be retracted or mid? what is the equivalent to off? TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive + Gains armTelescopingGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); + //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us /*public static enum ArmStates{ @@ -43,6 +50,14 @@ public ArmTelescopingSubsystem(){} public void init(){ telescopingMotor.setInverted(false); + + //configuring deadband + telescopingMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ + telescopingMotor.config_kF(Constants.kPIDLoopIdx, armTelescopingGains.kF, Constants.kTimeoutMs); + telescopingMotor.config_kP(Constants.kPIDLoopIdx, armTelescopingGains.kP, Constants.kTimeoutMs); + telescopingMotor.config_kI(Constants.kPIDLoopIdx, armTelescopingGains.kI, Constants.kTimeoutMs); + telescopingMotor.config_kD(Constants.kPIDLoopIdx, armTelescopingGains.kD, Constants.kTimeoutMs); } public void periodic(){//sam requests that we can operate arm length by stick on xbox diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index d9f4b43..c8cae2f 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -2,13 +2,26 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.NeutralMode; import frc.robot.Constants; +import frc.robot.Gains; + +import com.ctre.phoenix.motorcontrol.ControlMode; public class ElevatorSubsystem { public ElevatorSubsystem(){} + //these values are to be determined (untested) + double _kP = 1.0; + double _kI = 0.0; + double _kD = 0.0; + double _kF = 0.0; + int _kIzone = 0; + double _kPeakOutput = 0.0; + public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); - public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; + public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; //why is this here? + + Gains elevatorGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); public static enum ElevatorStates{ ZERO, @@ -20,17 +33,26 @@ public static enum ElevatorStates{ public void init(){ elevatorMotor.setInverted(false); elevatorMotor.setNeutralMode(NeutralMode.Brake); + + //configuring deadband + elevatorMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ + elevatorMotor.config_kF(Constants.kPIDLoopIdx, elevatorGains.kF, Constants.kTimeoutMs); + elevatorMotor.config_kP(Constants.kPIDLoopIdx, elevatorGains.kP, Constants.kTimeoutMs); + elevatorMotor.config_kI(Constants.kPIDLoopIdx, elevatorGains.kI, Constants.kTimeoutMs); + elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); } public void periodic(){ if (elevatorState == ElevatorStates.ZERO){ - + elevatorMotor.set(ControlMode.Position, 0); + elevatorMotor.set(ControlMode.PercentOutput, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - + elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ - + elevatorMotor.set(ControlMode.Position, 200); //change value once we know robot dimensions } else { - + elevatorMotor.set(ControlMode.Position, 300); //change value once we know robot dimensions } } From 323156c69d01300bbf6d8a2cf447e0f93ecfd422 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 21 Jan 2023 14:59:17 -0800 Subject: [PATCH 08/30] deleted gains in constants --- src/main/java/frc/robot/Constants.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 19e70b5..06d3ef9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,12 +66,6 @@ public final class Constants { public static final int kTimeoutMs = 1000; public static final int kPIDLoopIdx = 0;//not sure what this value does or if this value should be diff for rotation, telescoping, elevator - /** - * Gains used in Positon Closed Loop, to be adjusted accordingly - * Gains(kp, ki, kd, kf, izone, peak output); - */ - static final Gains kGains = new Gains(0.15, 0.0, 1.0, 0, 1.0); - /* Choose so that Talon does not report sensor out of phase */ public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. From ac08c80a129310bcce5b2f48e32d0a7f90963f89 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Mon, 23 Jan 2023 18:28:02 -0800 Subject: [PATCH 09/30] deleted kf cleaned up constants file --- src/main/java/frc/robot/Constants.java | 2 +- .../subsystems/ArmTelescopingSubsystem.java | 12 +-------- .../robot/subsystems/ElevatorSubsystem.java | 25 +++++++++---------- 3 files changed, 14 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 06d3ef9..6aa0c51 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,5 +69,5 @@ public final class Constants { /* Choose so that Talon does not report sensor out of phase */ public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. - public static double deadband = 0.5; //arbitrary + //public static double deadband = 0.5; //arbitrary } diff --git a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java index db3372c..416a7e2 100644 --- a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java @@ -12,7 +12,6 @@ public class ArmTelescopingSubsystem { double _kP = 1.0; double _kI = 0.0; double _kD = 0.0; - double _kF = 0.0; int _kIzone = 0; double _kPeakOutput = 0.0; @@ -20,18 +19,10 @@ public class ArmTelescopingSubsystem { TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive - Gains armTelescopingGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); + Gains armTelescopingGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us - /*public static enum ArmStates{ - OFF, //fully retracted - FULLY_EXTENDED, - LOW_ARM_LENGTH, // arm length when scoring on low node - MID_ARM_LENGTH, // also shelf height - HIGH_ARM_LENGTH; - }*/ - double extensionVal = 56.26;//this value should be the full extension length of the arm minus the length of it at zero public static enum TelescopingStates{ @@ -54,7 +45,6 @@ public void init(){ //configuring deadband telescopingMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ - telescopingMotor.config_kF(Constants.kPIDLoopIdx, armTelescopingGains.kF, Constants.kTimeoutMs); telescopingMotor.config_kP(Constants.kPIDLoopIdx, armTelescopingGains.kP, Constants.kTimeoutMs); telescopingMotor.config_kI(Constants.kPIDLoopIdx, armTelescopingGains.kI, Constants.kTimeoutMs); telescopingMotor.config_kD(Constants.kPIDLoopIdx, armTelescopingGains.kD, Constants.kTimeoutMs); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index c8cae2f..5785c24 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -8,23 +8,23 @@ public class ElevatorSubsystem { - public ElevatorSubsystem(){} - //these values are to be determined (untested) - double _kP = 1.0; - double _kI = 0.0; - double _kD = 0.0; - double _kF = 0.0; - int _kIzone = 0; - double _kPeakOutput = 0.0; + public double _kP = 1.0; + public double _kI = 0.0; + public double _kD = 0.0; + public int _kIzone = 0; + public double _kPeakOutput = 1.0; + + + public ElevatorSubsystem(){} public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; //why is this here? - Gains elevatorGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); - + public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); + public static enum ElevatorStates{ - ZERO, + ZERO, //janet doesn't know if we need this b/c low elevator height seems like the shortest one we'd need LOW_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, HIGH_ELEVATOR_HEIGHT; @@ -37,7 +37,6 @@ public void init(){ //configuring deadband elevatorMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ - elevatorMotor.config_kF(Constants.kPIDLoopIdx, elevatorGains.kF, Constants.kTimeoutMs); elevatorMotor.config_kP(Constants.kPIDLoopIdx, elevatorGains.kP, Constants.kTimeoutMs); elevatorMotor.config_kI(Constants.kPIDLoopIdx, elevatorGains.kI, Constants.kTimeoutMs); elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); @@ -46,7 +45,7 @@ public void init(){ public void periodic(){ if (elevatorState == ElevatorStates.ZERO){ elevatorMotor.set(ControlMode.Position, 0); - elevatorMotor.set(ControlMode.PercentOutput, 0); + //elevatorMotor.set(ControlMode.PercentOutput, 0); //pretty sure ControlMode.Position stops motor right when we get to setpoint } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ From 889c912f0b47e32bc72adaf0ec881c234c09b1d3 Mon Sep 17 00:00:00 2001 From: laurenmarymcdonald <68357627+laurenmarymcdonald@users.noreply.github.com> Date: Fri, 27 Jan 2023 16:08:11 -0800 Subject: [PATCH 10/30] elevator --- src/main/java/frc/robot/Gains.java | 5 ++--- .../java/frc/robot/subsystems/ArmRotationSubsystem.java | 4 +--- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 6 +----- 3 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Gains.java b/src/main/java/frc/robot/Gains.java index fcf204b..9ecbe2c 100644 --- a/src/main/java/frc/robot/Gains.java +++ b/src/main/java/frc/robot/Gains.java @@ -7,16 +7,15 @@ public class Gains { //Gain is a proportional value that shows the relationship public final double kP; public final double kI; public final double kD; - public final double kF; public final int kIzone; //can be used to auto clear the integral accumulator if the sensor pos is too far from the target. This prevent unstable oscillation if the kI is too large. Value is in sensor units. public final double kPeakOutput; //Absolute max motor output during closed-loop control modes only. A value of ‘1’ represents full output in both directions. - public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput){ + public Gains(double _kP, double _kI, double _kD, int _kIzone, double _kPeakOutput){ kP = _kP; kI = _kI; kD = _kD; - kF = _kF; + kIzone = _kIzone; kPeakOutput = _kPeakOutput; } diff --git a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java index aaf11a7..5d600f0 100644 --- a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java @@ -15,7 +15,6 @@ public class ArmRotationSubsystem { double _kP = 1.0; double _kI = 0.0; double _kD = 0.0; - double _kF = 0.0; int _kIzone = 0; double _kPeakOutput = 0.0; @@ -23,7 +22,7 @@ public class ArmRotationSubsystem { TalonFX armRotationMotor = new TalonFX(Constants.ARM_ROTATION_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive - Gains armRotationGains = new Gains(_kP, _kI, _kD, _kF, _kIzone, _kPeakOutput); + Gains armRotationGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ArmRotationStates{ @@ -48,7 +47,6 @@ public void init(){ //configuring deadband armRotationMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ - armRotationMotor.config_kF(Constants.kPIDLoopIdx, armRotationGains.kF, Constants.kTimeoutMs); armRotationMotor.config_kP(Constants.kPIDLoopIdx, armRotationGains.kP, Constants.kTimeoutMs); armRotationMotor.config_kI(Constants.kPIDLoopIdx, armRotationGains.kI, Constants.kTimeoutMs); armRotationMotor.config_kD(Constants.kPIDLoopIdx, armRotationGains.kD, Constants.kTimeoutMs); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 5785c24..4a0b280 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -15,11 +15,8 @@ public class ElevatorSubsystem { public int _kIzone = 0; public double _kPeakOutput = 1.0; - - public ElevatorSubsystem(){} - public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); - public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; //why is this here? + public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); @@ -45,7 +42,6 @@ public void init(){ public void periodic(){ if (elevatorState == ElevatorStates.ZERO){ elevatorMotor.set(ControlMode.Position, 0); - //elevatorMotor.set(ControlMode.PercentOutput, 0); //pretty sure ControlMode.Position stops motor right when we get to setpoint } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ From 813c8568757d97536051c21470c6c3601462b794 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Fri, 27 Jan 2023 17:31:52 -0800 Subject: [PATCH 11/30] made four bar linkage subsystem --- src/main/java/frc/robot/Constants.java | 12 +- .../robot/autonomous/AutonomousBaseMP.java | 134 ++++++++++++++++++ .../subsystems/FourBarLinkageSubsystem.java | 73 ++++++++++ 3 files changed, 215 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/autonomous/AutonomousBaseMP.java create mode 100644 src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6aa0c51..ae1a1d6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,12 @@ public final class Constants { public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 3; public static final double BACK_RIGHT_MODULE_STEER_OFFSET = -Math.toRadians(79.45); // FIXME Measure and set back right steer offset + public static final int TELESCOPING_MOTOR_ID = 7; + public static final int ARM_ROTATION_MOTOR_ID = 8; + public static final int ELEVATOR_CAN_ID = 9; + public static final int LEFT_LINK_CAN_ID = 10; + public static final int RIGHT_LINK_CAN_ID = 11; + public static final double DRIVE_MOTOR_MIN_VOLTAGE = 0.19; public static final double DRIVE_MOTOR_MAX_VOLTAGE = 0.8; public static final double STEER_MOTOR_MIN_VOLTAGE = 0.02; @@ -59,13 +65,11 @@ public final class Constants { public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder public static final double ARM_ROTATION_GEAR_RATIO = 0.0;//this is undecided as of Jan 20th - public static final int TELESCOPING_MOTOR_ID = 7; - public static final int ARM_ROTATION_MOTOR_ID = 8; - public static final int ELEVATOR_CAN_ID = 9; + public static final int kTimeoutMs = 1000; public static final int kPIDLoopIdx = 0;//not sure what this value does or if this value should be diff for rotation, telescoping, elevator - + /* Choose so that Talon does not report sensor out of phase */ public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. diff --git a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java new file mode 100644 index 0000000..a22fabd --- /dev/null +++ b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java @@ -0,0 +1,134 @@ +package frc.robot.autonomous; + +import java.util.ArrayList; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.geometry.*; +import frc.robot.Robot; +import frc.robot.subsystems.DrivetrainSubsystem; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; +import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; + +public class AutonomousBaseMP extends AutonomousBase{ + private double timeStart; + private double timeElapsed = 0; + private Trajectory trajectory1; + private Trajectory trajectory2; + private Trajectory trajectory3; + private HolonomicDriveController controller = new HolonomicDriveController( + new PIDController(1, 0, 0), new PIDController(1, 0, 0), + new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(6.28, Math.PI))); + // trapezoid profile takes in max rotation velocity and max rotation acceleration + // PIDController #1: the first arg rep how many m/s added in the x direction for every meter of error in the x direction + // PIDController #2 : the first arg rep how many m/s added in the y direction for every meter of error in the y direction + + private static DrivetrainSubsystem drivetrainSubsystem = Robot.m_drivetrainSubsystem; + + public AutonomousBaseMP(Trajectory trajectory1, Trajectory trajectory2, Trajectory trajectory3){ + this.trajectory1 = trajectory1; + this.trajectory2 = trajectory2; + this.trajectory3 = trajectory3; + } + + @Override + public void init(){ + timeStart = System.currentTimeMillis(); + } + + @Override + public void periodic(){ + timeElapsed = System.currentTimeMillis() - timeStart; + if (doing == Doing.TRAJECTORY1){ + followTrajectory(trajectory1); + if (trajectoryDone(trajectory1)){ + setDoing(Doing.TRAJECTORY2); + } + } else if (doing == Doing.TRAJECTORY2){ + followTrajectory(trajectory2); + if (trajectoryDone(trajectory2)){ + setDoing(Doing.TRAJECTORY3); + } + } else if (doing == Doing.TRAJECTORY3){ + followTrajectory(trajectory3); + if (trajectoryDone(trajectory3)){ + setDoing(Doing.STOP); + } + } else if (doing == Doing.PLACEHIGH){ + + } else if (doing == Doing.PICKUP){ + + } else if (doing == Doing.BALANCE){ + + } else { + drivetrainSubsystem.stopDrive(); + } + } + + public boolean trajectoryDone(Trajectory trajectory){ + double timeCheck = trajectory.getTotalTimeSeconds(); + Trajectory.State end = trajectory.sample(timeCheck); + if(Math.abs(end.poseMeters.getX() - DrivetrainSubsystem.m_pose.getX()) < 2 && + Math.abs(end.poseMeters.getY() - DrivetrainSubsystem.m_pose.getY()) < 2 && + Math.abs(end.poseMeters.getRotation().getDegrees() - DrivetrainSubsystem.m_pose.getRotation().getDegrees()) < 2){ + return true; + } + return false; + } + + public static enum Doing{ + TRAJECTORY1, + TRAJECTORY2, + TRAJECTORY3, + PLACEHIGH, + PICKUP, + BALANCE, + STOP; + } + public Doing doing = Doing.TRAJECTORY1; + + public void setDoing(Doing newDoing){ + doing = newDoing; + } + + public static Trajectory generateTrajectory(Pose2d starting, Pose2d ending, Translation2d interior1, Translation2d interior2, Translation2d interior3){ + ArrayList interiorWaypoints = new ArrayList(); + interiorWaypoints.add(interior1); + interiorWaypoints.add(interior2); + interiorWaypoints.add(interior3); + + SwerveDriveKinematicsConstraint swerveDriveKinematicsConstraint = new SwerveDriveKinematicsConstraint(drivetrainSubsystem.m_kinematics, DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); + MaxVelocityConstraint maxVelocityConstraint = new MaxVelocityConstraint(DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); + + TrajectoryConfig config = new TrajectoryConfig(4.96, 2.8); //we should maybe look into this further + config.addConstraint(swerveDriveKinematicsConstraint); + config.addConstraint(maxVelocityConstraint); + + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + starting, + interiorWaypoints, + ending, + config + ); + return trajectory; + } + + public void followTrajectory(Trajectory trajectory){ + Trajectory.State goal = trajectory.sample(timeElapsed); + + ChassisSpeeds adjustedSpeeds = controller.calculate( + DrivetrainSubsystem.m_pose, goal, Rotation2d.fromDegrees(0)); + + drivetrainSubsystem.setSpeed(adjustedSpeeds); + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java b/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java new file mode 100644 index 0000000..1f4b841 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java @@ -0,0 +1,73 @@ +package frc.robot.subsystems; +import frc.robot.Constants; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import frc.robot.Gains; + +public class FourBarLinkageSubsystem { + //these values are to be determined (untested) + public double _kP = 1.0; + public double _kI = 0.0; + public double _kD = 0.0; + public int _kIzone = 0; + public double _kPeakOutput = 1.0; + + public static TalonFX leftLinkMotor = new TalonFX(Constants.LEFT_LINK_CAN_ID); + public static TalonFX rightLinkMotor = new TalonFX(Constants.RIGHT_LINK_CAN_ID); + public static LinkStateHolder linkStateHolder = LinkStateHolder.ZERO_HEIGHT; + + public Gains linkGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); + + public static enum LinkStateHolder{ + ZERO_HEIGHT, + LOWEST_HEIGHT, + MID_HEIGHT, + HIGH_HEIGHT, + FULLY_EXTENDED + } + public void init(){ + leftLinkMotor.setInverted(false); + rightLinkMotor.setInverted(false); + leftLinkMotor.setNeutralMode(NeutralMode.Brake); + rightLinkMotor.setNeutralMode(NeutralMode.Brake); + + leftLinkMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + rightLinkMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + + leftLinkMotor.config_kP(Constants.kPIDLoopIdx, linkGains.kP, Constants.kTimeoutMs); + leftLinkMotor.config_kI(Constants.kPIDLoopIdx, linkGains.kI, Constants.kTimeoutMs); + leftLinkMotor.config_kD(Constants.kPIDLoopIdx, linkGains.kD, Constants.kTimeoutMs); + + rightLinkMotor.config_kP(Constants.kPIDLoopIdx, linkGains.kP, Constants.kTimeoutMs); + rightLinkMotor.config_kI(Constants.kPIDLoopIdx, linkGains.kI, Constants.kTimeoutMs); + rightLinkMotor.config_kD(Constants.kPIDLoopIdx, linkGains.kD, Constants.kTimeoutMs); + } + + public void periodic(){ + if(linkStateHolder == LinkStateHolder.ZERO_HEIGHT) { + leftLinkMotor.set(ControlMode.Position, 0); // the following if statements are not tested, and should probably be changed before testing(the positions) + rightLinkMotor.set(ControlMode.Position, 0); + } + else if(linkStateHolder == LinkStateHolder.LOWEST_HEIGHT) { + leftLinkMotor.set(ControlMode.Position, 0); + rightLinkMotor.set(ControlMode.Position, 0); + } + else if(linkStateHolder == LinkStateHolder.MID_HEIGHT) { + leftLinkMotor.set(ControlMode.Position, 0); + rightLinkMotor.set(ControlMode.Position, 0); + } + else if(linkStateHolder == LinkStateHolder.HIGH_HEIGHT) { + leftLinkMotor.set(ControlMode.Position, 0); + rightLinkMotor.set(ControlMode.Position, 0); + } + else{ + leftLinkMotor.set(ControlMode.Position, 0); + rightLinkMotor.set(ControlMode.Position, 0); + } + } + + public void setState(LinkStateHolder newLinkState){ + linkStateHolder = newLinkState; + } +} From d00e55cefedda5bb3d4407cb70bcbfa2498179c8 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Mon, 30 Jan 2023 18:30:05 -0800 Subject: [PATCH 12/30] deleted rotate and 4 bar linkage code, polished gains --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/OI.java | 1 - .../subsystems/ArmRotationSubsystem.java | 90 ------------------- .../robot/subsystems/ElevatorSubsystem.java | 6 +- .../subsystems/FourBarLinkageSubsystem.java | 73 --------------- 5 files changed, 6 insertions(+), 170 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae1a1d6..328b624 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,13 +62,13 @@ public final class Constants { public static final double GEAR_RATIO = 6.75; public static final double WHEEL_DIAMETER= 4; //inches public static final double TICKS_PER_REV = 2048; - public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder - public static final double ARM_ROTATION_GEAR_RATIO = 0.0;//this is undecided as of Jan 20th + public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; // check if this is right //talonfx drive encoder + public static final double ELEVATOR_GEAR_RATIO = 100.0; //says arya on jan 30th public static final int kTimeoutMs = 1000; - public static final int kPIDLoopIdx = 0;//not sure what this value does or if this value should be diff for rotation, telescoping, elevator + public static final int kPIDLoopIdx = 0; //not sure what this value does or if this value should be diff for rotation, telescoping, elevator /* Choose so that Talon does not report sensor out of phase */ public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 245a530..5c39bb3 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -8,6 +8,5 @@ public class OI { public static Button stop = Button.kB; public static Button armExtended = Button.kY;//button to fully extend to 90 degrees - public static Button swingThrough = Button.kX;//button to go through public static Button armControl = Button.kLeftStick;//stick to control arm length (any length from fully retracted to extended) } diff --git a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java b/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java deleted file mode 100644 index 5d600f0..0000000 --- a/src/main/java/frc/robot/subsystems/ArmRotationSubsystem.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import frc.robot.Constants; -import frc.robot.Gains;//i think this means we do not need to have our own gains file - -import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.math.controller.PIDController; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.BaseMotorController.*; - - -public class ArmRotationSubsystem { - //these values are to be determined (untested) - double _kP = 1.0; - double _kI = 0.0; - double _kD = 0.0; - int _kIzone = 0; - double _kPeakOutput = 0.0; - - public static ArmRotationStates rState = ArmRotationStates.ZERO;//should this be retracted or mid? what is the equivalent to off? - - TalonFX armRotationMotor = new TalonFX(Constants.ARM_ROTATION_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive - - Gains armRotationGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); - - - public static enum ArmRotationStates{ - ZERO, //vertical - NINETY, - MID_ROTATION_ANGLE, //off the shelf - LOW_ROTATION_ANGLE, - HIGH_ROTATION_ANGLE; - } - - public void setRState(ArmRotationStates newState){ - rState = newState; - } - - public ArmRotationSubsystem(){} - - public void init(){ - armRotationMotor.setInverted(false); - armRotationMotor.setNeutralMode(NeutralMode.Brake); - //need to confirm with kim that we can zero motor when arm is vertical, then have negative 90 and 90 respectively when it rotates - - //configuring deadband - armRotationMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); - /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ - armRotationMotor.config_kP(Constants.kPIDLoopIdx, armRotationGains.kP, Constants.kTimeoutMs); - armRotationMotor.config_kI(Constants.kPIDLoopIdx, armRotationGains.kI, Constants.kTimeoutMs); - armRotationMotor.config_kD(Constants.kPIDLoopIdx, armRotationGains.kD, Constants.kTimeoutMs); - } - - - public void periodic(){ - if (rState == ArmRotationStates.ZERO){ - armRotationMotor.set(ControlMode.Position, 0); - armRotationMotor.set(ControlMode.PercentOutput, 0); - } else if (rState == ArmRotationStates.NINETY){ - armRotationMotor.set(ControlMode.Position, 90); //the following values are arbitrary - } else if (rState == ArmRotationStates.LOW_ROTATION_ANGLE){ - armRotationMotor.set(ControlMode.Position, 8); - }else if (rState == ArmRotationStates.HIGH_ROTATION_ANGLE){ - armRotationMotor.set(ControlMode.Position, 5); - }else{ - armRotationMotor.set(ControlMode.Position, 15); - } - } - - - public void rotateArm(double armRotationSetpoint){//setpoint in radians - armRotationSetpoint = armRotationSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant - armRotationMotor.set(ControlMode.Position, armRotationSetpoint);//change to whatever motor we use for arm - } - - public double ticksToDegrees(double ticks){ - return(ticks * 360 * Constants.ARM_ROTATION_GEAR_RATIO / Constants.TICKS_PER_REV); - } - - public double getRotationAngle(){ - return ticksToDegrees(armRotationMotor.getSelectedSensorPosition());//returns current rotation angle - } - - //potentiometer? flight sensor? - //AD worried about slipping with encoder and gravity - //absolute tick count - //KBR says no worries - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 4a0b280..9aac87d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -21,7 +21,7 @@ public class ElevatorSubsystem { public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ElevatorStates{ - ZERO, //janet doesn't know if we need this b/c low elevator height seems like the shortest one we'd need + ZERO, LOW_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, HIGH_ELEVATOR_HEIGHT; @@ -33,7 +33,7 @@ public void init(){ //configuring deadband elevatorMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); - /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ + /* Config Position Closed Loop gains in slot0, typically kF stays zero. */ elevatorMotor.config_kP(Constants.kPIDLoopIdx, elevatorGains.kP, Constants.kTimeoutMs); elevatorMotor.config_kI(Constants.kPIDLoopIdx, elevatorGains.kI, Constants.kTimeoutMs); elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); @@ -45,7 +45,7 @@ public void periodic(){ } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, 200); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, 24.5); //change value once we know robot dimensions } else { elevatorMotor.set(ControlMode.Position, 300); //change value once we know robot dimensions } diff --git a/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java b/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java deleted file mode 100644 index 1f4b841..0000000 --- a/src/main/java/frc/robot/subsystems/FourBarLinkageSubsystem.java +++ /dev/null @@ -1,73 +0,0 @@ -package frc.robot.subsystems; -import frc.robot.Constants; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.ControlMode; -import frc.robot.Gains; - -public class FourBarLinkageSubsystem { - //these values are to be determined (untested) - public double _kP = 1.0; - public double _kI = 0.0; - public double _kD = 0.0; - public int _kIzone = 0; - public double _kPeakOutput = 1.0; - - public static TalonFX leftLinkMotor = new TalonFX(Constants.LEFT_LINK_CAN_ID); - public static TalonFX rightLinkMotor = new TalonFX(Constants.RIGHT_LINK_CAN_ID); - public static LinkStateHolder linkStateHolder = LinkStateHolder.ZERO_HEIGHT; - - public Gains linkGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); - - public static enum LinkStateHolder{ - ZERO_HEIGHT, - LOWEST_HEIGHT, - MID_HEIGHT, - HIGH_HEIGHT, - FULLY_EXTENDED - } - public void init(){ - leftLinkMotor.setInverted(false); - rightLinkMotor.setInverted(false); - leftLinkMotor.setNeutralMode(NeutralMode.Brake); - rightLinkMotor.setNeutralMode(NeutralMode.Brake); - - leftLinkMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); - rightLinkMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); - - leftLinkMotor.config_kP(Constants.kPIDLoopIdx, linkGains.kP, Constants.kTimeoutMs); - leftLinkMotor.config_kI(Constants.kPIDLoopIdx, linkGains.kI, Constants.kTimeoutMs); - leftLinkMotor.config_kD(Constants.kPIDLoopIdx, linkGains.kD, Constants.kTimeoutMs); - - rightLinkMotor.config_kP(Constants.kPIDLoopIdx, linkGains.kP, Constants.kTimeoutMs); - rightLinkMotor.config_kI(Constants.kPIDLoopIdx, linkGains.kI, Constants.kTimeoutMs); - rightLinkMotor.config_kD(Constants.kPIDLoopIdx, linkGains.kD, Constants.kTimeoutMs); - } - - public void periodic(){ - if(linkStateHolder == LinkStateHolder.ZERO_HEIGHT) { - leftLinkMotor.set(ControlMode.Position, 0); // the following if statements are not tested, and should probably be changed before testing(the positions) - rightLinkMotor.set(ControlMode.Position, 0); - } - else if(linkStateHolder == LinkStateHolder.LOWEST_HEIGHT) { - leftLinkMotor.set(ControlMode.Position, 0); - rightLinkMotor.set(ControlMode.Position, 0); - } - else if(linkStateHolder == LinkStateHolder.MID_HEIGHT) { - leftLinkMotor.set(ControlMode.Position, 0); - rightLinkMotor.set(ControlMode.Position, 0); - } - else if(linkStateHolder == LinkStateHolder.HIGH_HEIGHT) { - leftLinkMotor.set(ControlMode.Position, 0); - rightLinkMotor.set(ControlMode.Position, 0); - } - else{ - leftLinkMotor.set(ControlMode.Position, 0); - rightLinkMotor.set(ControlMode.Position, 0); - } - } - - public void setState(LinkStateHolder newLinkState){ - linkStateHolder = newLinkState; - } -} From ead7477d218142975fe8b0519f7538aa05e0ccd2 Mon Sep 17 00:00:00 2001 From: Kaylin Yip Date: Mon, 30 Jan 2023 20:55:59 -0800 Subject: [PATCH 13/30] transferred buttons from buttonOfficial branch --- src/main/java/frc/robot/OI.java | 44 +++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 5c39bb3..00dc3a8 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -1,12 +1,46 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj.XboxController; public class OI { - public static final XboxController m_controller = new XboxController(0); - public static Button stop = Button.kB; - public static Button armExtended = Button.kY;//button to fully extend to 90 degrees - public static Button armControl = Button.kLeftStick;//stick to control arm length (any length from fully retracted to extended) -} + public double intakeOn, intakeOff; + + public static final XboxController m_controller = new XboxController(0);//buttons/co-driver controller + public static final XboxController m_controller_two = new XboxController(1);//main driver/driving controller + + //public static Button armExtended = Button.kY;//button to fully extend to 90 degrees + //public static Button armControl = Button.kLeftStick;//stick to control arm length (any length from fully retracted to extended) + public static Button pitchPD = Button.kLeftBumper;//pitch pd for charge station + public static Button stop = Button.kRightBumper;//stop drive bumper + //public static Trigger leftTrigger = Trigger.; + + public static Button lowShelf = Button.kX; // low shelf height combo of elevator and arm angle + public static Button midShelf = Button.kA; // mid shelf + public static Button highShelf = Button.kB; // high shelf + public static Button turnNorth = Button.kY;//button to turn to true north + + + public static Button outtake = Button.kA; + + public void triggerPeriodic() { + intakeOn = triggerThreshold (m_controller.getLeftTriggerAxis()); + intakeOff = triggerThreshold (m_controller.getRightTriggerAxis()); + } + /*two total xbox controllers, one for driver, one for co-driver. 4 buttons on each, 2 joysticks, 2 triggers, 2 bumbers, 2 additional buttons at the back of the contoller, and possibly 4 directional arrow keys. All can be programmed for different settings as desired. + * buttons/joysticks to be coded: + * - make joystick for driving robot + * - outtake button + */ + + public static double triggerThreshold(double tValue) { + if (Math.abs(tValue) < 0.1) { //change this value to be more specific if wanted + return 0; + } else { + return 1 * tValue; + } + //trigger axis is bound by the range [0,1] not [-1,1] + } +} \ No newline at end of file From 86e110549b5b1622da274b17d91ae44ed75a1fbf Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Thu, 2 Feb 2023 18:08:48 -0800 Subject: [PATCH 14/30] changed ticks per inch constant --- src/main/java/frc/robot/Constants.java | 7 +- .../robot/subsystems/AprilTagSubsystem.java | 163 ++++++++++++++++++ 2 files changed, 166 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/AprilTagSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 328b624..ec85c50 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,12 +60,11 @@ public final class Constants { public static final double STEER_MOTOR_MIN_VOLTAGE = 0.02; public static final double STEER_MOTOR_MAX_VOLTAGE = 0.5; public static final double GEAR_RATIO = 6.75; - public static final double WHEEL_DIAMETER= 4; //inches + public static final double ELEVATOR_SPROCKET_DIAMETER= 1.05; //inches public static final double TICKS_PER_REV = 2048; - public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; // check if this is right //talonfx drive encoder - public static final double ELEVATOR_GEAR_RATIO = 100.0; //says arya on jan 30th + public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/ELEVATOR_SPROCKET_DIAMETER/Math.PI; // check if this is right //talonfx drive encoder + public static final double ELEVATOR_GEAR_RATIO = 25.0; //says arya on jan 30th // 25 motor spins for 1 shaft spin - public static final int kTimeoutMs = 1000; public static final int kPIDLoopIdx = 0; //not sure what this value does or if this value should be diff for rotation, telescoping, elevator diff --git a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java new file mode 100644 index 0000000..0c15eaf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java @@ -0,0 +1,163 @@ +package frc.robot.subsystems; +import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; + +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSink; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.autonomous.AutonomousBasePD; + +//import frc.robot.subsystems.AprilTagFieldEnum; + +public class AprilTagSubsystem { + + + //System.setProperty("java.library.path","C:\\Users\\Gatorbotics\\Downloads\\opencv\\build\\java\\x64") + //System.load("C:\\Users\\Gatorbotics\\Downloads\\opencv\\build\\java\\x64"); + + /*public enum AprilTagFields { + k2023ChargedUp("2023-chargedup.json"); + }*/ + + public static UsbCamera camera0; + //public static VideoSink sink; + private String family = "tag16h5"; + Mat source; + Mat grayMat; + CvSink cvSink; + AprilTagDetection[] detectedAprilTagsArray; + AprilTagDetection detectedAprilTag; + VideoSink server; + /*int aprilTagIds[]; + + float decisionMargin[]; + double centerX[]; + double centerY[];*/ + double drivetrainXPosition; + //Path path = Filesystem.getDefault().getPath("2023-chargedup.json"); + //public final String m_resourceFile; + + //CvSource outputStream; + + private static double tagSize = 4.0; //change to actual size(inches) + private static double fx;//need to add value + private static double fy;//need to add value + private static double cx;//need to add value + private static double cy;//need to add value + private static SwerveDrivePoseEstimator swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); //what arguments go here? + + public static enum AprilTagSequence{ + DETECT, + CORRECTPOSITION; + } + + private static AprilTagSequence states = AprilTagSequence.DETECT; + + public void setState(AprilTagSequence newState){ + states = newState; + } + + private static AprilTagDetector aprilTagDetector = new AprilTagDetector(); + + private static AutonomousBasePD autonomousBasePD = new AutonomousBasePD(); + + public final AprilTagPoseEstimator.Config aprilTagPoseEstimatorConfig = new Config(tagSize, fx, fy, cx, cy); + public AprilTagPoseEstimator aprilTagPoseEstimator = new AprilTagPoseEstimator(aprilTagPoseEstimatorConfig); + //AprilTagFieldLayout aprilTagFieldLayout = new AprilTagFieldLayout("src/main/deploy/2023-chargedup.json"); + //AprilTagFieldLayout secondAprilTagFieldLayout = new AprilTagFieldLayout(AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile)); + //AprilTagFieldLayout thirdAprilTagFieldLayout = new AprilTagFieldLayout("/edu/wpi/first/apriltag/2023-chargedup.json"); + //Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectory); + + public void init(){ + System.loadLibrary("opencv_java460"); + //camera0 = new UsbCamera("USB Camera 0", 0); + aprilTagDetector.addFamily(family, 0); //added 0 + camera0 = CameraServer.startAutomaticCapture(); //deleted 0 + server = CameraServer.getServer(); + //sink = CameraServer.getServer(); + source = new Mat(); + grayMat = new Mat(); + + + System.out.println("source from init: " + source); + //source.release(); + cvSink = CameraServer.getVideo(); + //outputStream = CameraServer.putVideo("camera stream", 320, 240); + } + + + //kaylin wrote this but probably did it wrong :) + public void addVisionToOdometry(){ + Transform3d aprilTagError = aprilTagPoseEstimator.estimate(detectedAprilTag);//april tag pose estimator in Transform 3d + Pose2d aprilTagPose2D = AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()-1].toPose2d();//pose 2d of the actual april tag + Rotation2d robotSubtractedAngle = Rotation2d.fromDegrees(aprilTagPose2D.getRotation().getDegrees()-aprilTagError.getRotation().toRotation2d().getDegrees());//angle needed to create pose 2d of robot position, don't know if toRotatation2D converts Rotation3D properly + Pose2d robotPost2DAprilTag = new Pose2d(aprilTagPose2D.getX()-aprilTagError.getX(), aprilTagPose2D.getY()-aprilTagError.getY(), robotSubtractedAngle); + swerveDrivePoseEstimator.addVisionMeasurement(robotPost2DAprilTag, Timer.getFPGATimestamp()); + } + + public void periodic(){ + if(states == AprilTagSequence.DETECT){ + detectTag(); + setState(AprilTagSequence.CORRECTPOSITION); + }else if(states == AprilTagSequence.CORRECTPOSITION){ + correctPosition(); + } + } + + private void detectTag(){ + long time = cvSink.grabFrame(source); + if(time ==0){ + System.out.println("failed to grab a frame"); + return; + } + Imgproc.cvtColor(source, grayMat,Imgproc.COLOR_BGR2GRAY); + System.out.println("value of source in periodic: " + source); + System.out.println(grayMat.empty()); + System.out.println(grayMat.width()); + System.out.println(grayMat.height()); + //outputStream.putFrame(source); + + detectedAprilTagsArray = aprilTagDetector.detect(grayMat); + detectedAprilTag = detectedAprilTagsArray[0]; + + + /*for(int i = 0; i < detectedAprilTags.length; i++){ + aprilTagIds[i] = detectedAprilTags[i].getId(); + decisionMargin[i] = detectedAprilTags[i].getDecisionMargin(); + centerX[i] = detectedAprilTags[i].getCenterX(); + centerY[i] = detectedAprilTags[i].getCenterY(); + }*/ + + + + System.out.println("Detected Apriltag: " + detectedAprilTag); + + } + + + private void correctPosition(){ + Pose2d prePose = autonomousBasePD.preDDD(DrivetrainSubsystem.m_pose, AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()].toPose2d()); + autonomousBasePD.driveDesiredDistance(prePose); + } + + + /*private void getError(){ + drivetrainXPosition = Robot.m_drivetrainSubsystem.m_pose.getX(); + for(int i = 0; i < aprilTagIds.length; i++){ + XError[i] = AprilTagLocation.aprilTagPoses[aprilTagIds[i]].getX() - drivetrainXPosition; + + //AprilTagLocation + } + }*/ +} From f25007cfd04de9d47691d916d46f25676fbd3fe1 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Thu, 2 Feb 2023 18:09:37 -0800 Subject: [PATCH 15/30] changed ticks per inch constant --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 9aac87d..5f0e1c6 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -45,12 +45,14 @@ public void periodic(){ } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, 24.5); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, 24.5 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } else { elevatorMotor.set(ControlMode.Position, 300); //change value once we know robot dimensions } } + + public void setState(ElevatorStates newElevatorState){ elevatorState = newElevatorState; } From 7d0313e8bb29478d268f38b8e059dcd73cc86748 Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Thu, 2 Feb 2023 19:07:51 -0800 Subject: [PATCH 16/30] tested elevator but zipties broke --- src/main/java/frc/robot/Robot.java | 35 +-- .../robot/autonomous/AutonomousBaseMP.java | 224 +++++++++--------- .../robot/autonomous/AutonomousBasePD.java | 2 +- .../robot/subsystems/AprilTagSubsystem.java | 26 +- .../robot/subsystems/ElevatorSubsystem.java | 8 +- 5 files changed, 149 insertions(+), 146 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 75ff7bc..52171eb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,12 +15,14 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates;; /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the * project. */ + public class Robot extends TimedRobot { private static final String kDefaultAuto = "Default"; private static final String kCustomAuto = "My Auto"; @@ -29,6 +31,8 @@ public class Robot extends TimedRobot { private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0); private static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); + public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + public static DrivetrainSubsystem getDrivetrainSubsystem(){ return m_drivetrainSubsystem; } @@ -71,26 +75,20 @@ public void robotPeriodic() { @Override public void autonomousInit() { autonomousBasePD.init(); - m_autoSelected = m_chooser.getSelected(); - m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); - System.out.println("Auto selected: " + m_autoSelected); + + elevatorSubsystem.init(); + // m_autoSelected = m_chooser.getSelected(); + // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); + // System.out.println("Auto selected: " + m_autoSelected); } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { - switch (m_autoSelected) { - case kCustomAuto: - // Put custom auto code here - break; - case kDefaultAuto: - default: - // Put default auto code here - break; - } - - autonomousBasePD.periodic(); + //autonomousBasePD.periodic(); + elevatorSubsystem.setState(ElevatorStates.MID_ELEVATOR_HEIGHT); + elevatorSubsystem.periodic(); //m_drivetrainSubsystem.drive(); } @@ -121,14 +119,17 @@ public void disabledPeriodic() {} /** This function is called once when test mode is enabled. */ @Override public void testInit() { - m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4)); - System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose); - autonomousBasePD.init(); + //m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4)); + //System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose); + //autonomousBasePD.init(); } /** This function is called periodically during test mode. */ @Override public void testPeriodic() { + elevatorSubsystem.setState(ElevatorStates.ZERO); + elevatorSubsystem.periodic(); + } /** This function is called once when the robot is first started up. */ diff --git a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java index a22fabd..01f3416 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java +++ b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java @@ -1,134 +1,134 @@ -package frc.robot.autonomous; +// package frc.robot.autonomous; -import java.util.ArrayList; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.geometry.*; -import frc.robot.Robot; -import frc.robot.subsystems.DrivetrainSubsystem; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; +// import java.util.ArrayList; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.controller.HolonomicDriveController; +// import edu.wpi.first.math.geometry.*; +// import frc.robot.Robot; +// import frc.robot.subsystems.DrivetrainSubsystem; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.trajectory.TrapezoidProfile; +// import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; -public class AutonomousBaseMP extends AutonomousBase{ - private double timeStart; - private double timeElapsed = 0; - private Trajectory trajectory1; - private Trajectory trajectory2; - private Trajectory trajectory3; - private HolonomicDriveController controller = new HolonomicDriveController( - new PIDController(1, 0, 0), new PIDController(1, 0, 0), - new ProfiledPIDController(1, 0, 0, - new TrapezoidProfile.Constraints(6.28, Math.PI))); - // trapezoid profile takes in max rotation velocity and max rotation acceleration - // PIDController #1: the first arg rep how many m/s added in the x direction for every meter of error in the x direction - // PIDController #2 : the first arg rep how many m/s added in the y direction for every meter of error in the y direction +// public class AutonomousBaseMP extends AutonomousBase{ +// private double timeStart; +// private double timeElapsed = 0; +// private Trajectory trajectory1; +// private Trajectory trajectory2; +// private Trajectory trajectory3; +// private HolonomicDriveController controller = new HolonomicDriveController( +// new PIDController(1, 0, 0), new PIDController(1, 0, 0), +// new ProfiledPIDController(1, 0, 0, +// new TrapezoidProfile.Constraints(6.28, Math.PI))); +// // trapezoid profile takes in max rotation velocity and max rotation acceleration +// // PIDController #1: the first arg rep how many m/s added in the x direction for every meter of error in the x direction +// // PIDController #2 : the first arg rep how many m/s added in the y direction for every meter of error in the y direction - private static DrivetrainSubsystem drivetrainSubsystem = Robot.m_drivetrainSubsystem; +// private static DrivetrainSubsystem drivetrainSubsystem = Robot.m_drivetrainSubsystem; - public AutonomousBaseMP(Trajectory trajectory1, Trajectory trajectory2, Trajectory trajectory3){ - this.trajectory1 = trajectory1; - this.trajectory2 = trajectory2; - this.trajectory3 = trajectory3; - } +// public AutonomousBaseMP(Trajectory trajectory1, Trajectory trajectory2, Trajectory trajectory3){ +// this.trajectory1 = trajectory1; +// this.trajectory2 = trajectory2; +// this.trajectory3 = trajectory3; +// } - @Override - public void init(){ - timeStart = System.currentTimeMillis(); - } +// @Override +// public void init(){ +// timeStart = System.currentTimeMillis(); +// } - @Override - public void periodic(){ - timeElapsed = System.currentTimeMillis() - timeStart; - if (doing == Doing.TRAJECTORY1){ - followTrajectory(trajectory1); - if (trajectoryDone(trajectory1)){ - setDoing(Doing.TRAJECTORY2); - } - } else if (doing == Doing.TRAJECTORY2){ - followTrajectory(trajectory2); - if (trajectoryDone(trajectory2)){ - setDoing(Doing.TRAJECTORY3); - } - } else if (doing == Doing.TRAJECTORY3){ - followTrajectory(trajectory3); - if (trajectoryDone(trajectory3)){ - setDoing(Doing.STOP); - } - } else if (doing == Doing.PLACEHIGH){ +// @Override +// public void periodic(){ +// timeElapsed = System.currentTimeMillis() - timeStart; +// if (doing == Doing.TRAJECTORY1){ +// followTrajectory(trajectory1); +// if (trajectoryDone(trajectory1)){ +// setDoing(Doing.TRAJECTORY2); +// } +// } else if (doing == Doing.TRAJECTORY2){ +// followTrajectory(trajectory2); +// if (trajectoryDone(trajectory2)){ +// setDoing(Doing.TRAJECTORY3); +// } +// } else if (doing == Doing.TRAJECTORY3){ +// followTrajectory(trajectory3); +// if (trajectoryDone(trajectory3)){ +// setDoing(Doing.STOP); +// } +// } else if (doing == Doing.PLACEHIGH){ - } else if (doing == Doing.PICKUP){ +// } else if (doing == Doing.PICKUP){ - } else if (doing == Doing.BALANCE){ +// } else if (doing == Doing.BALANCE){ - } else { - drivetrainSubsystem.stopDrive(); - } - } +// } else { +// drivetrainSubsystem.stopDrive(); +// } +// } - public boolean trajectoryDone(Trajectory trajectory){ - double timeCheck = trajectory.getTotalTimeSeconds(); - Trajectory.State end = trajectory.sample(timeCheck); - if(Math.abs(end.poseMeters.getX() - DrivetrainSubsystem.m_pose.getX()) < 2 && - Math.abs(end.poseMeters.getY() - DrivetrainSubsystem.m_pose.getY()) < 2 && - Math.abs(end.poseMeters.getRotation().getDegrees() - DrivetrainSubsystem.m_pose.getRotation().getDegrees()) < 2){ - return true; - } - return false; - } +// public boolean trajectoryDone(Trajectory trajectory){ +// double timeCheck = trajectory.getTotalTimeSeconds(); +// Trajectory.State end = trajectory.sample(timeCheck); +// if(Math.abs(end.poseMeters.getX() - DrivetrainSubsystem.m_pose.getX()) < 2 && +// Math.abs(end.poseMeters.getY() - DrivetrainSubsystem.m_pose.getY()) < 2 && +// Math.abs(end.poseMeters.getRotation().getDegrees() - DrivetrainSubsystem.m_pose.getRotation().getDegrees()) < 2){ +// return true; +// } +// return false; +// } - public static enum Doing{ - TRAJECTORY1, - TRAJECTORY2, - TRAJECTORY3, - PLACEHIGH, - PICKUP, - BALANCE, - STOP; - } - public Doing doing = Doing.TRAJECTORY1; +// public static enum Doing{ +// TRAJECTORY1, +// TRAJECTORY2, +// TRAJECTORY3, +// PLACEHIGH, +// PICKUP, +// BALANCE, +// STOP; +// } +// public Doing doing = Doing.TRAJECTORY1; - public void setDoing(Doing newDoing){ - doing = newDoing; - } +// public void setDoing(Doing newDoing){ +// doing = newDoing; +// } - public static Trajectory generateTrajectory(Pose2d starting, Pose2d ending, Translation2d interior1, Translation2d interior2, Translation2d interior3){ - ArrayList interiorWaypoints = new ArrayList(); - interiorWaypoints.add(interior1); - interiorWaypoints.add(interior2); - interiorWaypoints.add(interior3); +// public static Trajectory generateTrajectory(Pose2d starting, Pose2d ending, Translation2d interior1, Translation2d interior2, Translation2d interior3){ +// ArrayList interiorWaypoints = new ArrayList(); +// interiorWaypoints.add(interior1); +// interiorWaypoints.add(interior2); +// interiorWaypoints.add(interior3); - SwerveDriveKinematicsConstraint swerveDriveKinematicsConstraint = new SwerveDriveKinematicsConstraint(drivetrainSubsystem.m_kinematics, DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); - MaxVelocityConstraint maxVelocityConstraint = new MaxVelocityConstraint(DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); +// SwerveDriveKinematicsConstraint swerveDriveKinematicsConstraint = new SwerveDriveKinematicsConstraint(drivetrainSubsystem.m_kinematics, DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); +// MaxVelocityConstraint maxVelocityConstraint = new MaxVelocityConstraint(DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND); - TrajectoryConfig config = new TrajectoryConfig(4.96, 2.8); //we should maybe look into this further - config.addConstraint(swerveDriveKinematicsConstraint); - config.addConstraint(maxVelocityConstraint); +// TrajectoryConfig config = new TrajectoryConfig(4.96, 2.8); //we should maybe look into this further +// config.addConstraint(swerveDriveKinematicsConstraint); +// config.addConstraint(maxVelocityConstraint); - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - starting, - interiorWaypoints, - ending, - config - ); - return trajectory; - } +// Trajectory trajectory = TrajectoryGenerator.generateTrajectory( +// starting, +// interiorWaypoints, +// ending, +// config +// ); +// return trajectory; +// } - public void followTrajectory(Trajectory trajectory){ - Trajectory.State goal = trajectory.sample(timeElapsed); +// public void followTrajectory(Trajectory trajectory){ +// Trajectory.State goal = trajectory.sample(timeElapsed); - ChassisSpeeds adjustedSpeeds = controller.calculate( - DrivetrainSubsystem.m_pose, goal, Rotation2d.fromDegrees(0)); +// ChassisSpeeds adjustedSpeeds = controller.calculate( +// DrivetrainSubsystem.m_pose, goal, Rotation2d.fromDegrees(0)); - drivetrainSubsystem.setSpeed(adjustedSpeeds); - } +// drivetrainSubsystem.setSpeed(adjustedSpeeds); +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/AutonomousBasePD.java b/src/main/java/frc/robot/autonomous/AutonomousBasePD.java index 51f7d16..d8a8221 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousBasePD.java +++ b/src/main/java/frc/robot/autonomous/AutonomousBasePD.java @@ -51,7 +51,7 @@ public void init(){ directionController.setTolerance(DEADBAND); distanceController.setTolerance(DEADBAND*Constants.TICKS_PER_INCH); states = States.FIRST; - System.out.println("init!"); + System.out.println("auto init!"); } public static enum States{ diff --git a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java index 0c15eaf..b55594b 100644 --- a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java @@ -70,7 +70,7 @@ public void setState(AprilTagSequence newState){ private static AprilTagDetector aprilTagDetector = new AprilTagDetector(); - private static AutonomousBasePD autonomousBasePD = new AutonomousBasePD(); + //private static AutonomousBasePD autonomousBasePD = new AutonomousBasePD(); public final AprilTagPoseEstimator.Config aprilTagPoseEstimatorConfig = new Config(tagSize, fx, fy, cx, cy); public AprilTagPoseEstimator aprilTagPoseEstimator = new AprilTagPoseEstimator(aprilTagPoseEstimatorConfig); @@ -98,20 +98,20 @@ public void init(){ //kaylin wrote this but probably did it wrong :) - public void addVisionToOdometry(){ - Transform3d aprilTagError = aprilTagPoseEstimator.estimate(detectedAprilTag);//april tag pose estimator in Transform 3d - Pose2d aprilTagPose2D = AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()-1].toPose2d();//pose 2d of the actual april tag - Rotation2d robotSubtractedAngle = Rotation2d.fromDegrees(aprilTagPose2D.getRotation().getDegrees()-aprilTagError.getRotation().toRotation2d().getDegrees());//angle needed to create pose 2d of robot position, don't know if toRotatation2D converts Rotation3D properly - Pose2d robotPost2DAprilTag = new Pose2d(aprilTagPose2D.getX()-aprilTagError.getX(), aprilTagPose2D.getY()-aprilTagError.getY(), robotSubtractedAngle); - swerveDrivePoseEstimator.addVisionMeasurement(robotPost2DAprilTag, Timer.getFPGATimestamp()); - } + // public void addVisionToOdometry(){ + // Transform3d aprilTagError = aprilTagPoseEstimator.estimate(detectedAprilTag);//april tag pose estimator in Transform 3d + // Pose2d aprilTagPose2D = AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()-1].toPose2d();//pose 2d of the actual april tag + // Rotation2d robotSubtractedAngle = Rotation2d.fromDegrees(aprilTagPose2D.getRotation().getDegrees()-aprilTagError.getRotation().toRotation2d().getDegrees());//angle needed to create pose 2d of robot position, don't know if toRotatation2D converts Rotation3D properly + // Pose2d robotPost2DAprilTag = new Pose2d(aprilTagPose2D.getX()-aprilTagError.getX(), aprilTagPose2D.getY()-aprilTagError.getY(), robotSubtractedAngle); + // swerveDrivePoseEstimator.addVisionMeasurement(robotPost2DAprilTag, Timer.getFPGATimestamp()); + // } public void periodic(){ if(states == AprilTagSequence.DETECT){ detectTag(); setState(AprilTagSequence.CORRECTPOSITION); }else if(states == AprilTagSequence.CORRECTPOSITION){ - correctPosition(); + // correctPosition(); } } @@ -146,10 +146,10 @@ private void detectTag(){ } - private void correctPosition(){ - Pose2d prePose = autonomousBasePD.preDDD(DrivetrainSubsystem.m_pose, AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()].toPose2d()); - autonomousBasePD.driveDesiredDistance(prePose); - } + // private void correctPosition(){ + // Pose2d prePose = autonomousBasePD.preDDD(DrivetrainSubsystem.m_pose, AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()].toPose2d()); + // autonomousBasePD.driveDesiredDistance(prePose); + // } /*private void getError(){ diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 5f0e1c6..18c3efc 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -16,18 +16,19 @@ public class ElevatorSubsystem { public double _kPeakOutput = 1.0; public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); - public static ElevatorStates elevatorState = ElevatorStates.MID_ELEVATOR_HEIGHT; + public static ElevatorStates elevatorState = ElevatorStates.ZERO; public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ElevatorStates{ - ZERO, + ZERO, LOW_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, HIGH_ELEVATOR_HEIGHT; } public void init(){ + System.out.println("elevvator init!!!!"); elevatorMotor.setInverted(false); elevatorMotor.setNeutralMode(NeutralMode.Brake); @@ -40,7 +41,8 @@ public void init(){ } public void periodic(){ - if (elevatorState == ElevatorStates.ZERO){ + System.out.println("periodicQ!!!!!!!"); + if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions From 930bf33040b13d2fa02debc217108e38aa8811be Mon Sep 17 00:00:00 2001 From: Kaylin Yip Date: Thu, 2 Feb 2023 19:50:32 -0800 Subject: [PATCH 17/30] added 1/3 speed control for high elevator state in driveTeleop() --- .../frc/robot/subsystems/DrivetrainSubsystem.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 8392069..ccdc93b 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -25,6 +25,7 @@ import static frc.robot.Constants.*; import frc.robot.OI; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; public class DrivetrainSubsystem { @@ -90,6 +91,7 @@ public class DrivetrainSubsystem { //ChassisSpeeds takes in y velocity, x velocity, speed of rotation private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + private static ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); public DrivetrainSubsystem() { ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); @@ -229,6 +231,8 @@ public void setSpeed(ChassisSpeeds chassisSpeeds) { m_chassisSpeeds = chassisSpeeds; } + + // public Pose2d getCurrentPose(){ // return m_pose; // } @@ -243,6 +247,16 @@ public void driveTeleop(){ DoubleSupplier m_rotationSupplier = () -> -modifyAxis(OI.m_controller.getRightX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; //setting speed + if (elevatorSubsystem.elevatorState == ElevatorStates.HIGH_ELEVATOR_HEIGHT){ + setSpeed( + ChassisSpeeds.fromFieldRelativeSpeeds( + m_translationXSupplier.getAsDouble(), + m_translationYSupplier.getAsDouble(), + m_rotationSupplier.getAsDouble(), + getGyroscopeRotation() + ) + ); + } setSpeed( ChassisSpeeds.fromFieldRelativeSpeeds( m_translationXSupplier.getAsDouble(), From d03ae04d48806fda089039f37e7e5b190d32dd2a Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Fri, 3 Feb 2023 19:17:13 -0800 Subject: [PATCH 18/30] changed controlmode.position vals --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 4 ++-- .../robot/subsystems/ElevatorSubsystem.java | 18 ++++++++++++------ 4 files changed, 16 insertions(+), 10 deletions(-) diff --git a/build.gradle b/build.gradle index fadc9a4..4dc2bb0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.1.1" + id "edu.wpi.first.GradleRIO" version "2023.2.1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ec85c50..f51426b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,9 +62,9 @@ public final class Constants { public static final double GEAR_RATIO = 6.75; public static final double ELEVATOR_SPROCKET_DIAMETER= 1.05; //inches public static final double TICKS_PER_REV = 2048; - public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/ELEVATOR_SPROCKET_DIAMETER/Math.PI; // check if this is right //talonfx drive encoder public static final double ELEVATOR_GEAR_RATIO = 25.0; //says arya on jan 30th // 25 motor spins for 1 shaft spin + public static final double TICKS_PER_INCH = (TICKS_PER_REV*ELEVATOR_GEAR_RATIO)/(ELEVATOR_SPROCKET_DIAMETER*Math.PI); //talonfx drive encoder public static final int kTimeoutMs = 1000; public static final int kPIDLoopIdx = 0; //not sure what this value does or if this value should be diff for rotation, telescoping, elevator diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 52171eb..01c57dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -74,7 +74,7 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - autonomousBasePD.init(); + //autonomousBasePD.init(); elevatorSubsystem.init(); // m_autoSelected = m_chooser.getSelected(); @@ -87,7 +87,7 @@ public void autonomousInit() { public void autonomousPeriodic() { //autonomousBasePD.periodic(); - elevatorSubsystem.setState(ElevatorStates.MID_ELEVATOR_HEIGHT); + elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); elevatorSubsystem.periodic(); //m_drivetrainSubsystem.drive(); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 18c3efc..f561ae4 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -6,15 +6,21 @@ import com.ctre.phoenix.motorcontrol.ControlMode; +//NOTES ON MEASUREMENTS +//22.5inches on inside of metal frame that chain moves in +//10 in on moving mechanism thing + public class ElevatorSubsystem { //these values are to be determined (untested) - public double _kP = 1.0; + public double _kP = 0.05; public double _kI = 0.0; public double _kD = 0.0; public int _kIzone = 0; public double _kPeakOutput = 1.0; + public double scaleDown = 1.214; //this value was determined using tested values and plotted by lauren macdonald in loggerpro! + public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.ZERO; @@ -29,7 +35,7 @@ public static enum ElevatorStates{ public void init(){ System.out.println("elevvator init!!!!"); - elevatorMotor.setInverted(false); + elevatorMotor.setInverted(true); elevatorMotor.setNeutralMode(NeutralMode.Brake); //configuring deadband @@ -40,16 +46,16 @@ public void init(){ elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); } - public void periodic(){ + public void periodic(){//need to divide each value by System.out.println("periodicQ!!!!!!!"); if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, 100); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, (8/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, 24.5 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, (24.5/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } else { - elevatorMotor.set(ControlMode.Position, 300); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, 20 / scaleDown); //change value once we know robot dimensions } } From e36eca41c09511f4aa691e5217fcd8a1f9eea7c2 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 4 Feb 2023 12:17:16 -0800 Subject: [PATCH 19/30] added limit switch --- src/main/java/frc/robot/Constants.java | 2 ++ .../robot/subsystems/ElevatorSubsystem.java | 20 ++++++++++++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f51426b..ae8a330 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -73,4 +73,6 @@ public final class Constants { public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. //public static double deadband = 0.5; //arbitrary + + public static int limitSwitchPort = 1; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index f561ae4..107b109 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -3,6 +3,8 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import frc.robot.Constants; import frc.robot.Gains; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.Constants; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -23,10 +25,13 @@ public class ElevatorSubsystem { public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.ZERO; + + DigitalInput limit_switch = new DigitalInput(Constants.limitSwitchPort); public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ElevatorStates{ + STOP, ZERO, LOW_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, @@ -34,7 +39,7 @@ public static enum ElevatorStates{ } public void init(){ - System.out.println("elevvator init!!!!"); + System.out.println("elevator init!!!!"); elevatorMotor.setInverted(true); elevatorMotor.setNeutralMode(NeutralMode.Brake); @@ -47,8 +52,10 @@ public void init(){ } public void periodic(){//need to divide each value by - System.out.println("periodicQ!!!!!!!"); - if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down + System.out.println("periodic!!!!!!!"); + if (elevatorState == ElevatorStates.STOP){ //emergency stop + elevatorMotor.set(ControlMode.PercentOutput, 0); + }else if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, (8/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions @@ -57,9 +64,12 @@ public void periodic(){//need to divide each value by } else { elevatorMotor.set(ControlMode.Position, 20 / scaleDown); //change value once we know robot dimensions } - } - + if(limit_switch.get()){ + System.out.println("Limit switch worked"); + setState(ElevatorStates.STOP); + } + } public void setState(ElevatorStates newElevatorState){ elevatorState = newElevatorState; From 853061c197196aca72a297996fba2185d8c90513 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 4 Feb 2023 14:37:55 -0800 Subject: [PATCH 20/30] added bottom limit switch and stop command --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 7 ++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae8a330..912ff4c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -74,5 +74,6 @@ public final class Constants { //public static double deadband = 0.5; //arbitrary - public static int limitSwitchPort = 1; + public static int topLimitSwitchPort = 0; + public static int bottomLimitSwitchPort = 1; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 107b109..74d0fc4 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -26,7 +26,8 @@ public class ElevatorSubsystem { public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.ZERO; - DigitalInput limit_switch = new DigitalInput(Constants.limitSwitchPort); + DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort); + DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort); public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); @@ -65,9 +66,9 @@ public void periodic(){//need to divide each value by elevatorMotor.set(ControlMode.Position, 20 / scaleDown); //change value once we know robot dimensions } - if(limit_switch.get()){ - System.out.println("Limit switch worked"); + if(top_limit_switch.get() || bottom_limit_switch.get()){ setState(ElevatorStates.STOP); + System.out.println("Top/bottom limit switch triggered"); } } From 37c2c3cf6192691519bc53963f6247e702532aff Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Sat, 4 Feb 2023 15:30:08 -0800 Subject: [PATCH 21/30] changed some constants and added comments --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 912ff4c..931a094 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,10 +59,10 @@ public final class Constants { public static final double DRIVE_MOTOR_MAX_VOLTAGE = 0.8; public static final double STEER_MOTOR_MIN_VOLTAGE = 0.02; public static final double STEER_MOTOR_MAX_VOLTAGE = 0.5; - public static final double GEAR_RATIO = 6.75; + // public static final double GEAR_RATIO = 6.75; public static final double ELEVATOR_SPROCKET_DIAMETER= 1.05; //inches public static final double TICKS_PER_REV = 2048; - public static final double ELEVATOR_GEAR_RATIO = 25.0; //says arya on jan 30th // 25 motor spins for 1 shaft spin + public static final double ELEVATOR_GEAR_RATIO = 25.0; // 25 motor spins for 1 shaft spin public static final double TICKS_PER_INCH = (TICKS_PER_REV*ELEVATOR_GEAR_RATIO)/(ELEVATOR_SPROCKET_DIAMETER*Math.PI); //talonfx drive encoder @@ -74,6 +74,6 @@ public final class Constants { //public static double deadband = 0.5; //arbitrary - public static int topLimitSwitchPort = 0; + public static int topLimitSwitchPort = 2; public static int bottomLimitSwitchPort = 1; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 74d0fc4..f8acaa2 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -21,7 +21,7 @@ public class ElevatorSubsystem { public int _kIzone = 0; public double _kPeakOutput = 1.0; - public double scaleDown = 1.214; //this value was determined using tested values and plotted by lauren macdonald in loggerpro! + public double scaleDown = 1.214; //this value was determined using tested values and plotted by lauren mcdonald in loggerpro! public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.ZERO; @@ -32,7 +32,7 @@ public class ElevatorSubsystem { public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ElevatorStates{ - STOP, + STOP, //goes with limit switch ZERO, LOW_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, @@ -59,7 +59,7 @@ public void periodic(){//need to divide each value by }else if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, (8/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, (5/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, (24.5/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } else { From 6251060563c48ae5c9e5709a8c58309375d55944 Mon Sep 17 00:00:00 2001 From: 25laurenj <25ljene@castilleja.org> Date: Sat, 4 Feb 2023 17:29:29 -0800 Subject: [PATCH 22/30] deleted unnecessary import --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index f8acaa2..d622d7d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -4,7 +4,6 @@ import frc.robot.Constants; import frc.robot.Gains; import edu.wpi.first.wpilibj.DigitalInput; -import frc.robot.Constants; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -52,7 +51,7 @@ public void init(){ elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); } - public void periodic(){//need to divide each value by + public void periodic(){//still need to scale down the values or figure out why it is overshooting System.out.println("periodic!!!!!!!"); if (elevatorState == ElevatorStates.STOP){ //emergency stop elevatorMotor.set(ControlMode.PercentOutput, 0); From d040edf81f191c967e981ce613263cb7542bea42 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Sat, 11 Feb 2023 12:07:33 -0800 Subject: [PATCH 23/30] added scorelow score mid score high methods b/c deleted from buttonofficial --- .../robot/subsystems/DrivetrainSubsystem.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 8392069..482fa04 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -25,6 +25,8 @@ import static frc.robot.Constants.*; import frc.robot.OI; +import frc.robot.subsystems.ArmTelescopingSubsystem.TelescopingStates; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; public class DrivetrainSubsystem { @@ -90,6 +92,8 @@ public class DrivetrainSubsystem { //ChassisSpeeds takes in y velocity, x velocity, speed of rotation private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + private ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + private ArmTelescopingSubsystem armTelescopingSubsystem = new ArmTelescopingSubsystem(); public DrivetrainSubsystem() { ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); @@ -303,6 +307,22 @@ public double getMedian(double[] num){ } } + public void scoreLow(){ + elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.LOW_ARM_LENGTH); + } + + public void scoreMid(){ + elevatorSubsystem.setState(ElevatorStates.MID_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.MID_ARM_LENGTH); + } + + public void scoreHigh(){ + elevatorSubsystem.setState(ElevatorStates.HIGH_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.HIGH_ARM_LENGTH); + } + + //AUTO AND FAILSAFE public void stopDrive() { setSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); From 667d9b598b00b88ece0c3aee27802ec2392d91f0 Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Sat, 11 Feb 2023 12:49:07 -0800 Subject: [PATCH 24/30] added scorelow scoremid scorehigh methods b/c deleted from buttonofficial --- .../robot/autonomous/AutonomousBaseTimed.java | 30 +++++++++++++++ .../frc/robot/autonomous/Trajectories.java | 38 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 src/main/java/frc/robot/autonomous/AutonomousBaseTimed.java create mode 100644 src/main/java/frc/robot/autonomous/Trajectories.java diff --git a/src/main/java/frc/robot/autonomous/AutonomousBaseTimed.java b/src/main/java/frc/robot/autonomous/AutonomousBaseTimed.java new file mode 100644 index 0000000..5b691ce --- /dev/null +++ b/src/main/java/frc/robot/autonomous/AutonomousBaseTimed.java @@ -0,0 +1,30 @@ +package frc.robot.autonomous; + +import frc.robot.Robot; +import frc.robot.subsystems.*; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class AutonomousBaseTimed extends AutonomousBase{ + DrivetrainSubsystem drivetrainSubsystem = Robot.m_drivetrainSubsystem; + private double timeStart; + private double timeElapsed = 0; + final double target = 5.0; //units in seconds + + @Override + public void init(){ + drivetrainSubsystem.resetOdometry(new Pose2d()); + timeStart = System.currentTimeMillis(); + } + + @Override + public void periodic(){ + timeElapsed = (System.currentTimeMillis() - timeStart)/ 1000; + if(timeElapsed < target){ + drivetrainSubsystem.setSpeed(new ChassisSpeeds(0.0, 0.3, 0.0)); + drivetrainSubsystem.drive(); + } else { + drivetrainSubsystem.stopDrive(); + } + } +} diff --git a/src/main/java/frc/robot/autonomous/Trajectories.java b/src/main/java/frc/robot/autonomous/Trajectories.java new file mode 100644 index 0000000..83a8e74 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/Trajectories.java @@ -0,0 +1,38 @@ +package frc.robot.autonomous; + +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +//IN METERS! +public class Trajectories{ + + public static Trajectory uno = AutonomousBaseMP.generateTrajectory( + new Pose2d(), + new Pose2d(2.0, 0.0, new Rotation2d(0)), + new Translation2d(1.0, 0.0), + new Translation2d(1.2, 0.0), + new Translation2d(1.4, 0.0) + ); + + public static Trajectory dos = AutonomousBaseMP.generateTrajectory( + new Pose2d(), + new Pose2d(1.5, 2.3, new Rotation2d(0)), + new Translation2d(.2, .5), + new Translation2d(.9, 1.5), + new Translation2d(1.5, 2.0) + ); + + public static Trajectory tres = AutonomousBaseMP.generateTrajectory( + new Pose2d(), + new Pose2d(2.0, -1.0, new Rotation2d(0)), + new Translation2d(.2, -.4), + new Translation2d(.9, -.6), + new Translation2d(1.5, -.8) + ); + + +} \ No newline at end of file From 8adc6c12096651c339d771bce3f595b6be56394a Mon Sep 17 00:00:00 2001 From: janetmeng <23jmeng@castilleja.org> Date: Mon, 13 Feb 2023 18:31:44 -0800 Subject: [PATCH 25/30] added shelf arm length --- .../robot/subsystems/ElevatorSubsystem.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index d622d7d..68a259d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -23,7 +23,7 @@ public class ElevatorSubsystem { public double scaleDown = 1.214; //this value was determined using tested values and plotted by lauren mcdonald in loggerpro! public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); - public static ElevatorStates elevatorState = ElevatorStates.ZERO; + public static ElevatorStates elevatorState = ElevatorStates.LOW_ELEVATOR_HEIGHT; DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort); DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort); @@ -32,8 +32,8 @@ public class ElevatorSubsystem { public static enum ElevatorStates{ STOP, //goes with limit switch - ZERO, LOW_ELEVATOR_HEIGHT, + SHELF_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, HIGH_ELEVATOR_HEIGHT; } @@ -52,17 +52,17 @@ public void init(){ } public void periodic(){//still need to scale down the values or figure out why it is overshooting - System.out.println("periodic!!!!!!!"); + System.out.println("elevator periodic!!!!!!!"); if (elevatorState == ElevatorStates.STOP){ //emergency stop elevatorMotor.set(ControlMode.PercentOutput, 0); - }else if (elevatorState == ElevatorStates.ZERO){ //facing forward, turning clockwise = going down - elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, (5/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, 0 * Constants.TICKS_PER_INCH); //official 2/13 + } else if (elevatorState == ElevatorStates.SHELF_ELEVATOR_HEIGHT) { + elevatorMotor.set(ControlMode.Position, 39 * Constants.TICKS_PER_INCH); //oficial 2/13 } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, (24.5/scaleDown) * Constants.TICKS_PER_INCH); //change value once we know robot dimensions - } else { - elevatorMotor.set(ControlMode.Position, 20 / scaleDown); //change value once we know robot dimensions + elevatorMotor.set(ControlMode.Position, 40 * Constants.TICKS_PER_INCH); //official 2/13 + } else { //high elevator height + elevatorMotor.set(ControlMode.Position, 48 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } if(top_limit_switch.get() || bottom_limit_switch.get()){ From 7f4021a0d3bed64e4fc4e71e9f2fc3ccb22903a2 Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Tue, 28 Feb 2023 12:45:57 -0800 Subject: [PATCH 26/30] not my changes not my monkeys --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 57 +++++++++---------- .../robot/autonomous/AutonomousBaseMP.java | 0 .../frc/robot/autonomous/Trajectories.java | 42 +++++++------- .../robot/subsystems/ElevatorSubsystem.java | 23 ++++---- 5 files changed, 61 insertions(+), 63 deletions(-) delete mode 100644 src/main/java/frc/robot/autonomous/AutonomousBaseMP.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 420279a..0e02246 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,7 +59,7 @@ public final class Constants { public static final int TELESCOPING_MOTOR_ID = 7; public static final int ARM_ROTATION_MOTOR_ID = 8; - public static final int ELEVATOR_CAN_ID = 9; + public static final int ELEVATOR_CAN_ID = 28; public static final int LEFT_LINK_CAN_ID = 10; public static final int RIGHT_LINK_CAN_ID = 11; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e7793dc..6baaf78 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.autonomous.*; import frc.robot.Constants; - +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; import com.fasterxml.jackson.core.sym.Name; import edu.wpi.first.math.controller.PIDController; @@ -36,19 +36,16 @@ * project. */ -public class Robot extends TimedRobot { +/*public class Robot extends TimedRobot { private static final String kDefaultAuto = "Default"; private static final String kCustomAuto = "My Auto"; private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0); - private static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); - - public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + //private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0); + public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); - public static DrivetrainSubsystem getDrivetrainSubsystem(){ - return m_drivetrainSubsystem; - } + +*/ //private AutonomousBasePD mScore = new AutonomousBasePD(new Translation2d(222.037, 0), new Translation2d(135.091, -41.307), new Translation2d(0, -44.163), new Translation2d(222.894, -50.377), new Translation2d(0, -65.388), new Translation2d(0, -65.388)); @@ -63,8 +60,8 @@ public class Robot extends TimedRobot { private AutonomousBasePD placeTwoEngage = new AutonomousBasePD(new Pose2d(223.014, 16.468, new Rotation2d(0)), new Pose2d(0, 22.683, new Rotation2d(0)), new Pose2d(135.615, 25.539, new Rotation2d(0)), new Pose2d(222.191, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0))); private AutonomousBaseTimed timedPath = new AutonomousBaseTimed(); private AutonomousBasePD testPath = new AutonomousBasePD(new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0))); - private AutonomousBaseMP motionProfiling = new AutonomousBaseMP(Trajectories.uno, Trajectories.dos, Trajectories.tres); - + public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + //above coordinates didn't work private AutonomousBasePD leave = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0))); private AutonomousBasePD placeTwoEngageReal = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(257.650, 64.004, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0))); @@ -72,7 +69,6 @@ public class Robot extends TimedRobot { //these paths score 3 balls without touching the charge station, requires 7 Pose2ds! private AutonomousBasePD threeUnderChargeStation = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(56.222, 43.068, new Rotation2d(0)), new Pose2d(197.484,45.934, new Rotation2d(0)), new Pose2d(279.077, 85.622, new Rotation2d(0)), new Pose2d(197.484,40.000, new Rotation2d(0)), new Pose2d(56.154,66.117, new Rotation2d(0))); private AutonomousBasePD threeAboveChargeStation = new AutonomousBasePD(new Pose2d(56.069, 200.046, new Rotation2d(0)), new Pose2d(278.999, 180.683, new Rotation2d(0)), new Pose2d(56.069, 174.725, new Rotation2d(0)), new Pose2d(207.006, 174.725, new Rotation2d(0)), new Pose2d(278.006, 133.515, new Rotation2d(0)), new Pose2d(200.552, 185.151, new Rotation2d(0)), new Pose2d(57.062, 154.368, new Rotation2d(0))); - public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); //if anything breaks in the future it might be this private final Field2d m_field = new Field2d(); @@ -82,6 +78,9 @@ public class Robot extends TimedRobot { public static GenericEntry kI; public static GenericEntry kD; + public static DrivetrainSubsystem getDrivetrainSubsystem(){ + return m_drivetrainSubsystem; + } /** * This function is run when the robot is first started up and should be used for any @@ -90,25 +89,23 @@ public class Robot extends TimedRobot { @Override public void robotInit() { //creates options for different autopaths, names are placeholders - System.out.println("#I'm Awake"); - m_chooser.setDefaultOption("Default Auto", testPath); - m_chooser.addOption("My Auto 1", noGo); - m_chooser.addOption("My Auto 2",placeNLeave); - m_chooser.addOption("My Auto 3", antiCharge); - m_chooser.addOption("My Auto 4", antiChargeOpposite); + // System.out.println("#I'm Awake"); + // m_chooser.setDefaultOption("Default Auto", testPath); + // m_chooser.addOption("My Auto 1", noGo); + // m_chooser.addOption("My Auto 2",placeNLeave); + // m_chooser.addOption("My Auto 3", antiCharge); + // m_chooser.addOption("My Auto 4", antiChargeOpposite); //m_chooser.addOption(name: "My Auto 5", engageCharge); - // m_chooser.addOption(name: "My Auto 6",placeTwoEngage); - m_chooser.addOption("My Auto timed", timedPath); - m_chooser.addOption("Motion profiling path", motionProfiling); + //m_chooser.addOption(name: "My Auto 6",placeTwoEngage); + //m_chooser.addOption("My Auto timed", timedPath); + //m_chooser.addOption("Motion profiling path", motionProfiling); SmartDashboard.putData("Auto choices", m_chooser); - ShuffleboardTab tab = DrivetrainSubsystem.tab; - kP = tab.add("Auto kP", 0.1).getEntry(); - kI = tab.add("Auto kI", 0.0).getEntry(); - kD = tab.add("Auto kD", 0.0).getEntry(); - - + // ShuffleboardTab tab = DrivetrainSubsystem.tab; + // kP = tab.add("Auto kP", 0.1).getEntry(); + // kI = tab.add("Auto kI", 0.0).getEntry(); + // kD = tab.add("Auto kD", 0.0).getEntry(); } /** @@ -153,7 +150,7 @@ public void autonomousPeriodic() { //autonomousBasePD.periodic(); - elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); + elevatorSubsystem.setState(ElevatorStates.SHELF_ELEVATOR_HEIGHT); elevatorSubsystem.periodic(); //m_drivetrainSubsystem.drive(); } @@ -205,9 +202,9 @@ public void testPeriodic() { elevatorSubsystem.periodic(); /*intial test!*/ - m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0)); + // m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0)); - m_drivetrainSubsystem.drive(); + // m_drivetrainSubsystem.drive(); } /** This function is called once when the robot is first started up. */ diff --git a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/autonomous/Trajectories.java b/src/main/java/frc/robot/autonomous/Trajectories.java index 83a8e74..062578b 100644 --- a/src/main/java/frc/robot/autonomous/Trajectories.java +++ b/src/main/java/frc/robot/autonomous/Trajectories.java @@ -10,29 +10,29 @@ //IN METERS! public class Trajectories{ - public static Trajectory uno = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(2.0, 0.0, new Rotation2d(0)), - new Translation2d(1.0, 0.0), - new Translation2d(1.2, 0.0), - new Translation2d(1.4, 0.0) - ); + // public static Trajectory uno = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(2.0, 0.0, new Rotation2d(0)), + // new Translation2d(1.0, 0.0), + // new Translation2d(1.2, 0.0), + // new Translation2d(1.4, 0.0) + // ); - public static Trajectory dos = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(1.5, 2.3, new Rotation2d(0)), - new Translation2d(.2, .5), - new Translation2d(.9, 1.5), - new Translation2d(1.5, 2.0) - ); + // public static Trajectory dos = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(1.5, 2.3, new Rotation2d(0)), + // new Translation2d(.2, .5), + // new Translation2d(.9, 1.5), + // new Translation2d(1.5, 2.0) + // ); - public static Trajectory tres = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(2.0, -1.0, new Rotation2d(0)), - new Translation2d(.2, -.4), - new Translation2d(.9, -.6), - new Translation2d(1.5, -.8) - ); + // public static Trajectory tres = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(2.0, -1.0, new Rotation2d(0)), + // new Translation2d(.2, -.4), + // new Translation2d(.9, -.6), + // new Translation2d(1.5, -.8) + // ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 68a259d..ac0a443 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -25,13 +25,13 @@ public class ElevatorSubsystem { public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.LOW_ELEVATOR_HEIGHT; - DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort); - DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort); + // DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort); + // DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort); public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); public static enum ElevatorStates{ - STOP, //goes with limit switch + ZERO, LOW_ELEVATOR_HEIGHT, SHELF_ELEVATOR_HEIGHT, MID_ELEVATOR_HEIGHT, @@ -40,7 +40,7 @@ public static enum ElevatorStates{ public void init(){ System.out.println("elevator init!!!!"); - elevatorMotor.setInverted(true); + elevatorMotor.setInverted(false); // looking from the front of the robot, clockwise is false (: elevatorMotor.setNeutralMode(NeutralMode.Brake); //configuring deadband @@ -53,10 +53,11 @@ public void init(){ public void periodic(){//still need to scale down the values or figure out why it is overshooting System.out.println("elevator periodic!!!!!!!"); - if (elevatorState == ElevatorStates.STOP){ //emergency stop - elevatorMotor.set(ControlMode.PercentOutput, 0); + if (elevatorState == ElevatorStates.ZERO){ //emergency stop + elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - elevatorMotor.set(ControlMode.Position, 0 * Constants.TICKS_PER_INCH); //official 2/13 + System.out.println("low elevator height"); + elevatorMotor.set(ControlMode.Position, 5 * Constants.TICKS_PER_INCH); //official 2/13 } else if (elevatorState == ElevatorStates.SHELF_ELEVATOR_HEIGHT) { elevatorMotor.set(ControlMode.Position, 39 * Constants.TICKS_PER_INCH); //oficial 2/13 } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ @@ -65,10 +66,10 @@ public void periodic(){//still need to scale down the values or figure out why i elevatorMotor.set(ControlMode.Position, 48 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions } - if(top_limit_switch.get() || bottom_limit_switch.get()){ - setState(ElevatorStates.STOP); - System.out.println("Top/bottom limit switch triggered"); - } + // if(top_limit_switch.get() || bottom_limit_switch.get()){ + // setState(ElevatorStates.STOP); + // System.out.println("Top/bottom limit switch triggered"); + // } } public void setState(ElevatorStates newElevatorState){ From f9e1b4d779fc72237da816ab56c9f7901920b802 Mon Sep 17 00:00:00 2001 From: Kira Libby Robertson <25krobertson@castilleja.org> Date: Tue, 28 Feb 2023 19:02:25 -0800 Subject: [PATCH 27/30] cleaned up code, mostly deleting unused imports, and comments about unsure variables --- src/main/java/frc/robot/Robot.java | 33 +----------------------------- 1 file changed, 1 insertion(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6baaf78..60a19d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,28 +6,14 @@ import edu.wpi.first.wpilibj.TimedRobot; import frc.robot.subsystems.*; -import frc.robot.OI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.autonomous.*; -import frc.robot.Constants; import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; -import com.fasterxml.jackson.core.sym.Name; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTable.*; -import edu.wpi.first.math.geometry.Rotation2d; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -43,33 +29,16 @@ private final SendableChooser m_chooser = new SendableChooser<>(); //private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0); public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); - */ //private AutonomousBasePD mScore = new AutonomousBasePD(new Translation2d(222.037, 0), new Translation2d(135.091, -41.307), new Translation2d(0, -44.163), new Translation2d(222.894, -50.377), new Translation2d(0, -65.388), new Translation2d(0, -65.388)); public class Robot extends TimedRobot { - private AutonomousBase m_autoSelected; private final SendableChooser m_chooser = new SendableChooser(); - private AutonomousBasePD noGo = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0))); - private AutonomousBasePD placeNLeave = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0))); - private AutonomousBasePD antiCharge = new AutonomousBasePD(new Pose2d(86.840, -45.282, new Rotation2d(0)), new Pose2d(221.978, 19.463, new Rotation2d(0)), new Pose2d(135.091, -19.421, new Rotation2d(0)), new Pose2d(0, -22.277, new Rotation2d(0)), new Pose2d(222.491, -28.492, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0))); - private AutonomousBasePD antiChargeOpposite = new AutonomousBasePD(new Pose2d(86.840, 45.282, new Rotation2d(0)), new Pose2d(221.978, -19.463, new Rotation2d(0)), new Pose2d(135.091, 19.421, new Rotation2d(0)), new Pose2d(0, 22.277, new Rotation2d(0)), new Pose2d(222.491, 28.492, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0))); - private AutonomousBasePD engageCharge = new AutonomousBasePD(new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0))); - private AutonomousBasePD placeTwoEngage = new AutonomousBasePD(new Pose2d(223.014, 16.468, new Rotation2d(0)), new Pose2d(0, 22.683, new Rotation2d(0)), new Pose2d(135.615, 25.539, new Rotation2d(0)), new Pose2d(222.191, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0))); - private AutonomousBaseTimed timedPath = new AutonomousBaseTimed(); - private AutonomousBasePD testPath = new AutonomousBasePD(new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0))); - public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); - //above coordinates didn't work - private AutonomousBasePD leave = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0))); - private AutonomousBasePD placeTwoEngageReal = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(257.650, 64.004, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0))); + public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); - //these paths score 3 balls without touching the charge station, requires 7 Pose2ds! - private AutonomousBasePD threeUnderChargeStation = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(56.222, 43.068, new Rotation2d(0)), new Pose2d(197.484,45.934, new Rotation2d(0)), new Pose2d(279.077, 85.622, new Rotation2d(0)), new Pose2d(197.484,40.000, new Rotation2d(0)), new Pose2d(56.154,66.117, new Rotation2d(0))); - private AutonomousBasePD threeAboveChargeStation = new AutonomousBasePD(new Pose2d(56.069, 200.046, new Rotation2d(0)), new Pose2d(278.999, 180.683, new Rotation2d(0)), new Pose2d(56.069, 174.725, new Rotation2d(0)), new Pose2d(207.006, 174.725, new Rotation2d(0)), new Pose2d(278.006, 133.515, new Rotation2d(0)), new Pose2d(200.552, 185.151, new Rotation2d(0)), new Pose2d(57.062, 154.368, new Rotation2d(0))); - public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); //if anything breaks in the future it might be this private final Field2d m_field = new Field2d(); ChassisSpeeds m_ChassisSpeeds; From 6e026912b0bb1d9a4271b18306ec1c16c51f2f1d Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Fri, 3 Mar 2023 18:28:08 -0800 Subject: [PATCH 28/30] changed elevator can id to 25 --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e02246..420279a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,7 +59,7 @@ public final class Constants { public static final int TELESCOPING_MOTOR_ID = 7; public static final int ARM_ROTATION_MOTOR_ID = 8; - public static final int ELEVATOR_CAN_ID = 28; + public static final int ELEVATOR_CAN_ID = 9; public static final int LEFT_LINK_CAN_ID = 10; public static final int RIGHT_LINK_CAN_ID = 11; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6baaf78..ffaff96 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -150,7 +150,7 @@ public void autonomousPeriodic() { //autonomousBasePD.periodic(); - elevatorSubsystem.setState(ElevatorStates.SHELF_ELEVATOR_HEIGHT); + elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); elevatorSubsystem.periodic(); //m_drivetrainSubsystem.drive(); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index ac0a443..64592b6 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -14,7 +14,7 @@ public class ElevatorSubsystem { //these values are to be determined (untested) - public double _kP = 0.05; + public double _kP = 0.005; public double _kI = 0.0; public double _kD = 0.0; public int _kIzone = 0; @@ -40,7 +40,7 @@ public static enum ElevatorStates{ public void init(){ System.out.println("elevator init!!!!"); - elevatorMotor.setInverted(false); // looking from the front of the robot, clockwise is false (: + elevatorMotor.setInverted(true); // looking from the front of the robot, clockwise is false (: elevatorMotor.setNeutralMode(NeutralMode.Brake); //configuring deadband From bbb14d567374d69b28583001b576acb651217c34 Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Fri, 3 Mar 2023 19:31:08 -0800 Subject: [PATCH 29/30] tested elevator --- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/ArmTelescopingSubsystem.java | 155 ++++++++++++------ .../robot/subsystems/ElevatorSubsystem.java | 12 +- 3 files changed, 111 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9c4d5a1..60a19d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -119,7 +119,7 @@ public void autonomousPeriodic() { //autonomousBasePD.periodic(); - elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); + elevatorSubsystem.setState(ElevatorStates.SHELF_ELEVATOR_HEIGHT); elevatorSubsystem.periodic(); //m_drivetrainSubsystem.drive(); } diff --git a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java index 416a7e2..6a9dc27 100644 --- a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java @@ -1,90 +1,141 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import frc.robot.Constants; -import frc.robot.Gains; +import edu.wpi.first.math.controller.PIDController; +import frc.robot.Constants; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; -public class ArmTelescopingSubsystem { +import frc.robot.Gains; - //these values are to be determined (untested) - double _kP = 1.0; - double _kI = 0.0; - double _kD = 0.0; - int _kIzone = 0; - double _kPeakOutput = 0.0; +public class ArmTelescopingSubsystem { + //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); public static TelescopingStates tState = TelescopingStates.RETRACTED;//should this be retracted or mid? what is the equivalent to off? - TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID);//maybe this motor should be renamed to make it more descriptive - - Gains armTelescopingGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); + public TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID); + private double startTime; + private double desiredInches; + private double desiredTicks; + public double tareEncoder; + //telescopingMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us - //armMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + public int _kIzone = 0; + public double _kPeakOutput = 1.0; - double extensionVal = 56.26;//this value should be the full extension length of the arm minus the length of it at zero + double telescopeKP = 0.05; + double telescopeKD = 0; + double telescopeKI = 0; + private int deadband = 25000; + //public PIDController telescopeController = new PIDController(telescopeKP, telescopeKI, telescopeKD) + //public Gains telescopeGains = new Gains(telescopeKP, telescopeKI, telescopeKD, _kIzone, _kPeakOutput); public static enum TelescopingStates{ - RETRACTED, - FULLY_EXTENDED, + RETRACTED, //zero LOW_ARM_LENGTH, + SHELF_ARM_LENGTH, MID_ARM_LENGTH, HIGH_ARM_LENGTH; } - - public void setTState(TelescopingStates newState){ - tState = newState; - } - - public ArmTelescopingSubsystem(){} public void init(){ - telescopingMotor.setInverted(false); + System.out.println("telescoping init!! :)"); + telescopingMotor.setInverted(false); //forward = clockwise, changed on 2/9 + telescopingMotor.setNeutralMode(NeutralMode.Brake); //configuring deadband telescopingMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); - /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */ - telescopingMotor.config_kP(Constants.kPIDLoopIdx, armTelescopingGains.kP, Constants.kTimeoutMs); - telescopingMotor.config_kI(Constants.kPIDLoopIdx, armTelescopingGains.kI, Constants.kTimeoutMs); - telescopingMotor.config_kD(Constants.kPIDLoopIdx, armTelescopingGains.kD, Constants.kTimeoutMs); + telescopingMotor.config_kP(0, telescopeKP); + telescopingMotor.config_kI(0, telescopeKI); + telescopingMotor.config_kP(0, telescopeKD); } public void periodic(){//sam requests that we can operate arm length by stick on xbox + //telescopingMotor.set(ControlMode.PercentOutput, 0.2); + System.out.println("current telescoping arm motor position:" + telescopingMotor.getSelectedSensorPosition()); if (tState == TelescopingStates.RETRACTED){ telescopingMotor.set(ControlMode.Position, 0); - telescopingMotor.set(ControlMode.PercentOutput, 0); - } else if (tState == TelescopingStates.FULLY_EXTENDED){ - telescopingMotor.set(ControlMode.Position, extensionVal); //confirmed + desiredInches = 0; + desiredTicks = 0; + telescopeDeadband(); } else if (tState == TelescopingStates.LOW_ARM_LENGTH){ - telescopingMotor.set(ControlMode.Position, 8); // replace position value w low length + desiredInches = 3; //official 2/13 + // determineRightTicks(); + System.out.println("desired ticks: " + desiredTicks); + telescopingMotor.set(ControlMode.Position, desiredTicks); + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); + System.out.println(telescopingMotor.getClosedLoopError()); + System.out.println(telescopingMotor.getMotorOutputPercent()); + System.out.println(telescopingMotor.getClosedLoopTarget()); + + } else if (tState == TelescopingStates.SHELF_ARM_LENGTH){ + desiredInches = 6; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); }else if (tState == TelescopingStates.MID_ARM_LENGTH){ - telescopingMotor.set(ControlMode.Position, 5); // goes with 90 degrees rotation // replace position value w mid length - }else{ - telescopingMotor.set(ControlMode.Position, 15); // replace position value w high length + desiredInches = 23; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); + }else{ //high arm length + desiredInches = 32; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); + telescopeDeadband(); } } - - public void moveArm(double armSetpoint){//setpoint in inches - armSetpoint = armSetpoint * Constants.TICKS_PER_INCH;//will probably have to change because wheel diameter in this constant is not relevant - telescopingMotor.set(ControlMode.Position, armSetpoint);//change to whatever motor we use for arm + // public void determineRightTicks(){ + // if (telescopingMotor.getSelectedSensorPosition() < 2 * Constants.UNDER_TWO_TICKS_PER_INCH){ + // desiredTicks = 2 * Constants.UNDER_TWO_TICKS_PER_INCH; + // } else{ + // desiredTicks = (desiredInches-2) * Constants.OVER_TWO_TICKS_PER_INCH; + // } + // } + public void telescopeDeadband(){ + if (Math.abs(desiredTicks - telescopingMotor.getSelectedSensorPosition()) < deadband){ + telescopingMotor.set(ControlMode.PercentOutput, 0); + System.out.println("STOPPED"); + } } - + public double getArmPosition(){ return telescopingMotor.getSelectedSensorPosition(); - } - - //public void armPID(double armLengthSetpoint){ - // armLengthController.setSetpoint(armLengthSetpoint); - //double output = armLengthController.calculate(getArmLength(), armLengthSetpoint); - //System.out.println("output: " + output); - //armMotor.set(ControlMode.PercentOutput, output);//written as _talon.set(TalonFXControlMode.PercentOutput, leftYstick); in documentation. leftystick is joy.gety - //} - - // if (Math.abs(leftYstick) < 0.10/*this number should be changed through testing*/) { - // /* Within 10% of zero */ - // leftYstick = 0;//left joystick y axis in documentation. assign later - // } + } + + public void timedMoveArm(double time, boolean forwards){ //time in seconds + System.out.println("moving arm >:)"); + System.out.println("start time: " + startTime); + double milliTime = time * 1000; + System.out.println("milli time: " + milliTime); + if(forwards == true){ + while(System.currentTimeMillis() - startTime <= milliTime){ + telescopingMotor.set(ControlMode.PercentOutput,0.2); + } + telescopingMotor.set(ControlMode.PercentOutput,0); + }else{ + while(System.currentTimeMillis() - startTime <= milliTime){ + telescopingMotor.set(ControlMode.PercentOutput,-0.2); + } + telescopingMotor.set(ControlMode.PercentOutput,0); + } + } + + public void setTState(TelescopingStates newState){ + tState = newState; + } + + public void tareEncoder() { + tareEncoder = telescopingMotor.getSelectedSensorPosition(); + + } + public double getTicks() { + return telescopingMotor.getSelectedSensorPosition() - tareEncoder; + } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 64592b6..8fbbaf3 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -14,7 +14,7 @@ public class ElevatorSubsystem { //these values are to be determined (untested) - public double _kP = 0.005; + public double _kP = 0.05; public double _kI = 0.0; public double _kD = 0.0; public int _kIzone = 0; @@ -40,7 +40,7 @@ public static enum ElevatorStates{ public void init(){ System.out.println("elevator init!!!!"); - elevatorMotor.setInverted(true); // looking from the front of the robot, clockwise is false (: + elevatorMotor.setInverted(false); // looking from the front of the robot, clockwise is false (: elevatorMotor.setNeutralMode(NeutralMode.Brake); //configuring deadband @@ -56,10 +56,12 @@ public void periodic(){//still need to scale down the values or figure out why i if (elevatorState == ElevatorStates.ZERO){ //emergency stop elevatorMotor.set(ControlMode.Position, 0); } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ - System.out.println("low elevator height"); - elevatorMotor.set(ControlMode.Position, 5 * Constants.TICKS_PER_INCH); //official 2/13 + elevatorMotor.set(ControlMode.Position, 5 * Constants.TICKS_PER_INCH); //official 2/13 is 5 } else if (elevatorState == ElevatorStates.SHELF_ELEVATOR_HEIGHT) { - elevatorMotor.set(ControlMode.Position, 39 * Constants.TICKS_PER_INCH); //oficial 2/13 + elevatorMotor.set(ControlMode.Position, 30 * Constants.TICKS_PER_INCH); //oficial 2/13 is 39 + System.out.println("desired ticks: " + 30*Constants.TICKS_PER_INCH); + System.out.println("current ticks: " + elevatorMotor.getSelectedSensorPosition()); + System.out.println("error: " + (30*Constants.TICKS_PER_INCH - elevatorMotor.getSelectedSensorPosition())); } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ elevatorMotor.set(ControlMode.Position, 40 * Constants.TICKS_PER_INCH); //official 2/13 } else { //high elevator height From 7415567b78ef01350fdce21937dde93bd91d0442 Mon Sep 17 00:00:00 2001 From: Gatorbotics Date: Sat, 4 Mar 2023 13:58:25 -0800 Subject: [PATCH 30/30] set sensor position to 0 in test init() --- src/main/java/frc/robot/Robot.java | 6 +++--- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 3 --- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 60a19d4..d8405be 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -157,7 +157,7 @@ public void disabledPeriodic() {} /** This function is called once when test mode is enabled. */ @Override public void testInit() { - + ElevatorSubsystem.elevatorMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); //m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4)); //System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose); //autonomousBasePD.init(); @@ -167,8 +167,8 @@ public void testInit() { @Override public void testPeriodic() { - elevatorSubsystem.setState(ElevatorStates.ZERO); - elevatorSubsystem.periodic(); + //elevatorSubsystem.setState(ElevatorStates.ZERO); + //elevatorSubsystem.periodic(); /*intial test!*/ // m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0)); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 8fbbaf3..0fb4a4c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -13,15 +13,12 @@ public class ElevatorSubsystem { - //these values are to be determined (untested) public double _kP = 0.05; public double _kI = 0.0; public double _kD = 0.0; public int _kIzone = 0; public double _kPeakOutput = 1.0; - public double scaleDown = 1.214; //this value was determined using tested values and plotted by lauren mcdonald in loggerpro! - public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); public static ElevatorStates elevatorState = ElevatorStates.LOW_ELEVATOR_HEIGHT;