diff --git a/Constants.java b/Constants.java index 3d8edea..88a42cf 100644 --- a/Constants.java +++ b/Constants.java @@ -22,6 +22,24 @@ public static final class ArmMoveConstants{ public static final int CLAW_CONE_POSITION = 100;//13; 100 Goes right to the max set in armConstants public static final int CLAW_CUBE_POSITION = 5; } + public static final class DriveConstants { + public static final int MOTORLEFT1PORT = 0; + public static final int MOTORLEFT2PORT = 1; + public static final int MOTORRIGHT1PORT = 2; + public static final int MOTORRIGHT2PORT = 3; + + public static final int[] ENCODERPORTSLEFT = {0, 1}; + public static final int[] ENCODERPORTSRIGHT = {2, 3}; + public static final boolean ENCODERREVERSEDLEFT = false; + public static final boolean ENCODERREVERSEDRIGHT= true; + + public static final double ADAPTIVESTEERINGSENSITIVITY = 0.002; + + public static final int ENCODERTICKSPERREV= 1024; + + public static final double ENCODERDISTANCEPERPULSEINCHES =3.0; + + } diff --git a/Controllers/PersonalizedController.java b/Controllers/PersonalizedController.java index 861e088..799f165 100644 --- a/Controllers/PersonalizedController.java +++ b/Controllers/PersonalizedController.java @@ -3,6 +3,7 @@ public interface PersonalizedController { PersonalizedController getcontroller(); int enableFineControlButton(); + int enableDriveButton(); int goToHighPosButton(); int goToMedPosButton(); int goToGroundPickupPosButton(); diff --git a/Controllers/ps4Brandon.java b/Controllers/ps4Brandon.java index bf79718..1da059f 100644 --- a/Controllers/ps4Brandon.java +++ b/Controllers/ps4Brandon.java @@ -53,6 +53,10 @@ public PersonalizedController getcontroller() { public int enableFineControlButton() { return Button.kL1.value; } + @Override + public int enableDriveButton() { + return Button.kR1.value; + } @Override public boolean povPressed() { diff --git a/Controllers/xBoxBrandon.java b/Controllers/xBoxBrandon.java index 2b5f412..978b567 100644 --- a/Controllers/xBoxBrandon.java +++ b/Controllers/xBoxBrandon.java @@ -51,6 +51,10 @@ public int enableFineControlButton() { return buttonMappings[0]; } @Override + public int enableDriveButton() { + return XboxController.Button.kRightBumper.value; + } + @Override public int goToHighPosButton() { return buttonMappings[1]; } @@ -84,9 +88,9 @@ public int shiftHighButton() { public int openClawButton() { return buttonMappings[8]; } - //@Override + //@Override public int closeClawCubeButton() { - return buttonMappings[9]; + return buttonMappings[9]; } //@Override public int closeClawConeButton() { diff --git a/RobotContainer.java b/RobotContainer.java index 37b2745..726cf5d 100644 --- a/RobotContainer.java +++ b/RobotContainer.java @@ -7,7 +7,10 @@ import frc.robot.Controllers.*; import frc.robot.commands.*; import frc.robot.Constants.ArmMoveConstants; +import frc.robot.subsystems.LimelightSubsystem; import frc.robot.subsystems.ArmSubsystem.*; +import frc.robot.subsystems.DriveSubsystem.DriveControlCommand; +import frc.robot.subsystems.DriveSubsystem.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -20,7 +23,8 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - + static final DriveSubsystem m_DriveSubsystem = new DriveSubsystem(); + static final LimelightSubsystem m_LimelightSubsystem = new LimelightSubsystem(); static final ArmSubsystem m_ArmSubsystem = new ArmSubsystem(); //private final ps4Brandon m_PersonalizedController =new ps4Brandon(0); static final xBoxBrandon m_PersonalizedController =new xBoxBrandon(0); @@ -51,6 +55,9 @@ private void configureBindings() { new JoystickButton(m_PersonalizedController, m_PersonalizedController.enableFineControlButton()) .whileTrue(new ArmFineControlCommand(m_ArmSubsystem,m_PersonalizedController)); + + new JoystickButton(m_PersonalizedController, m_PersonalizedController.enableDriveButton()) + .whileTrue(new DriveControlCommand(m_DriveSubsystem, m_PersonalizedController)); new JoystickButton(m_PersonalizedController, m_PersonalizedController.goToHomeButton())// go to home (arm doesn't fall when turned off) .onTrue(new ArmFollowLineCommand(m_ArmSubsystem, 12, 1, 15)); @@ -86,11 +93,11 @@ private void configureBindings() { // new JoystickButton(m_PersonalizedController, m_PersonalizedController.openClawButton()) // .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_OPEN_POSITION)); - new JoystickButton(m_PersonalizedController, m_PersonalizedController.openClawButton()) + new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton()) .onTrue(MultiLine.MultiLineHHTest(m_ArmSubsystem)); - new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton()) - .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CUBE_POSITION)); + // new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton()) + // .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CUBE_POSITION)); new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawConeButton()) .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CONE_POSITION)); @@ -98,6 +105,14 @@ private void configureBindings() { } private void setDefaultCommands(){ + m_LimelightSubsystem.setDefaultCommand( + m_LimelightSubsystem.checkForTargetsCommand() + ); + + m_DriveSubsystem.setDefaultCommand( + m_DriveSubsystem.arcadeDriveSquaredCommand( + () -> 0, () -> 0) + ); m_ArmSubsystem.setDefaultCommand( m_ArmSubsystem.RunJointsToSetAnglesCommand() diff --git a/commands/Autos.java b/commands/Autos.java new file mode 100644 index 0000000..906c9fc --- /dev/null +++ b/commands/Autos.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.ArmSubsystem.ArmSubsystem; +import frc.robot.subsystems.DriveSubsystem.DriveSubsystem; +//import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public final class Autos { + /** Example static factory for an autonomous command. */ + public static Command exampleAutoDrive(DriveSubsystem subsystem) { + return Commands.sequence(subsystem.driveDistanceCommand(3, .5), subsystem.driveDistanceCommand(-3, 0.5)); + } + + public static Command exampleAutoArm(ArmSubsystem subsystem) { + return Commands.sequence(subsystem.RunJointsToThetaRZCommand(0,22,10), subsystem.RunJointsToThetaRZCommand(10,20,20)); + } + + private Autos() { + throw new UnsupportedOperationException("This is a utility class!"); + } +} diff --git a/commands/FollowTargets.java b/commands/FollowTargets.java new file mode 100644 index 0000000..18de264 --- /dev/null +++ b/commands/FollowTargets.java @@ -0,0 +1,52 @@ +package frc.robot.commands; + +import frc.robot.subsystems.LimelightSubsystem; +import frc.robot.subsystems.DriveSubsystem.DriveSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class FollowTargets extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private DriveSubsystem m_DriveSubsystem; + private LimelightSubsystem m_LimelightSubsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public FollowTargets(DriveSubsystem subsystem_drive, LimelightSubsystem subsystem_limelight) { + m_DriveSubsystem = subsystem_drive; + m_LimelightSubsystem= subsystem_limelight; + // // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem_drive); + // + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // Use addRequirements() here to declare subsystem dependencies. + double targetPosition = m_LimelightSubsystem.getTargets()[0]; + //double rot = Math.min(Math.max((targetPosition/5)*(Math.abs(targetPosition)/5),-0.8),0.6);//limit speed + double rot = Math.min(Math.max((targetPosition/Math.abs(targetPosition))*Math.sqrt(Math.abs(targetPosition/20)),-0.8),0.8); + m_DriveSubsystem.arcadeDriveSquared(0.0,-rot); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + } + diff --git a/subsystems/ArmSubsystem/ArmSubsystem.java b/subsystems/ArmSubsystem/ArmSubsystem.java index ed6df80..bb470de 100644 --- a/subsystems/ArmSubsystem/ArmSubsystem.java +++ b/subsystems/ArmSubsystem/ArmSubsystem.java @@ -15,43 +15,62 @@ public class ArmSubsystem extends SubsystemBase { // Define all of the Joints - // public Joint(int deviceID,Boolean limitSwitchDirection, - // double l_max_output, double l_min_output, - // double l_homingSpeed, int l_startingAngle, - // int l_maxAngle, int l_minAngle, - // int l_homePositionAngle, double l_degreesPerRev){ - private static Joint ShoulderJoint = new Joint(ArmConstants.SHOULDER_MOTOR_ID, ArmConstants.SHOULDER_LIMIT_SWITCH_DIRECTION, - ArmConstants.SHOULDER_JOINT_SPEED,ArmConstants.SHOULDER_JOINT_SPEED, - ArmConstants.SHOULDER_HOMING_SPEED, ArmConstants.THETA1_START_OFFSET, - ArmConstants.THETA1_MAX, ArmConstants.THETA1_MIN, - ArmConstants.THETA1_HOMED_OFFSET, ArmConstants.SHOULDER_DEGREES_PER_REVOLUTION, - false); + private static Joint ShoulderJoint = + new Joint(ArmConstants.SHOULDER_MOTOR_ID, + ArmConstants.SHOULDER_LIMIT_SWITCH_DIRECTION, + ArmConstants.SHOULDER_JOINT_SPEED, + ArmConstants.SHOULDER_JOINT_SPEED, + ArmConstants.SHOULDER_HOMING_SPEED, + ArmConstants.THETA1_START_OFFSET, + ArmConstants.THETA1_MAX, + ArmConstants.THETA1_MIN, + ArmConstants.THETA1_HOMED_OFFSET, + ArmConstants.SHOULDER_DEGREES_PER_REVOLUTION, + false); - private static Joint ElbowJoint = new Joint(ArmConstants.ELBOW_MOTOR_ID, ArmConstants.ELBOW_LIMIT_SWITCH_DIRECTION, - ArmConstants.ELBOW_JOINT_SPEED,ArmConstants.ELBOW_JOINT_SPEED, - ArmConstants.ELBOW_HOMING_SPEED, ArmConstants.THETA2_START_OFFSET, - ArmConstants.THETA2_MAX, ArmConstants.THETA2_MIN, - ArmConstants.THETA2_HOMED_OFFSET, ArmConstants.ELBOW_DEGREES_PER_REVOLUTION, - false); - - private static Joint AzimuthJoint = new Joint(ArmConstants.AZIMUTH_MOTOR_ID, ArmConstants.AZIMUTH_LIMIT_SWITCH_DIRECTION, - ArmConstants.AZIMUTH_JOINT_SPEED,ArmConstants.AZIMUTH_JOINT_SPEED, - ArmConstants.AZIMUTH_HOMING_SPEED, ArmConstants.AZIMUTH_START_OFFSET, - ArmConstants.AZIMUTH_MAX, ArmConstants.AZIMUTH_MIN, - ArmConstants.AZIMUTH_HOMED_OFFSET, ArmConstants.AZIMUTH_DEGREES_PER_REVOLUTION, - true); - - private static Joint ClawJoint = new Joint(ArmConstants.CLAW_MOTOR_ID, ArmConstants.CLAW_LIMIT_SWITCH_DIRECTION, - 0.5,ArmConstants.CLAW_JOINT_SPEED,//OVERIDE MAX SPEED FOR CLOSING - ArmConstants.CLAW_HOMING_SPEED, ArmConstants.CLAW_START_OFFSET, - ArmConstants.CLAW_MAX, ArmConstants.CLAW_MIN, - ArmConstants.CLAW_HOMED_OFFSET, ArmConstants.CLAW_DEGREES_PER_REVOLUTION, - false); - - private static double azimuthAngle = 0; + private static Joint ElbowJoint + = new Joint(ArmConstants.ELBOW_MOTOR_ID, + ArmConstants.ELBOW_LIMIT_SWITCH_DIRECTION, + ArmConstants.ELBOW_JOINT_SPEED, + ArmConstants.ELBOW_JOINT_SPEED, + ArmConstants.ELBOW_HOMING_SPEED, + ArmConstants.THETA2_START_OFFSET, + ArmConstants.THETA2_MAX, + ArmConstants.THETA2_MIN, + ArmConstants.THETA2_HOMED_OFFSET, + ArmConstants.ELBOW_DEGREES_PER_REVOLUTION, + false); + + private static Joint AzimuthJoint + = new Joint(ArmConstants.AZIMUTH_MOTOR_ID, + ArmConstants.AZIMUTH_LIMIT_SWITCH_DIRECTION, + ArmConstants.AZIMUTH_JOINT_SPEED, + ArmConstants.AZIMUTH_JOINT_SPEED, + ArmConstants.AZIMUTH_HOMING_SPEED, + ArmConstants.AZIMUTH_START_OFFSET, + ArmConstants.AZIMUTH_MAX, + ArmConstants.AZIMUTH_MIN, + ArmConstants.AZIMUTH_HOMED_OFFSET, + ArmConstants.AZIMUTH_DEGREES_PER_REVOLUTION, + true); + + private static Joint ClawJoint + = new Joint(ArmConstants.CLAW_MOTOR_ID, + ArmConstants.CLAW_LIMIT_SWITCH_DIRECTION, + 0.5, + ArmConstants.CLAW_JOINT_SPEED,//OVERIDE MAX SPEED FOR CLOSING + ArmConstants.CLAW_HOMING_SPEED, + ArmConstants.CLAW_START_OFFSET, + ArmConstants.CLAW_MAX, + ArmConstants.CLAW_MIN, + ArmConstants.CLAW_HOMED_OFFSET, + ArmConstants.CLAW_DEGREES_PER_REVOLUTION, + false); + + private static double azimuthAngle = 0; private static double ShoulderAngle = 100; - private static double elbowAngle =25; - private static double clawPosition =0; + private static double elbowAngle = 25; + private static double clawPosition = 0; public void initialize() { //force claw and azimuth into homed condition since they have no limit switch @@ -64,26 +83,26 @@ public double[] getArmAngles() { return l_currentPos; } public double[] getArmPositionThetaRZ() { - // Calculations done in Radians for 2 segment arm of equal segment lengths - //Length from shoulder to claw - double l_Hypotenus = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180); - //intetior angle between humerous and claw + double l_shoulderToClaw = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180); + + // Intetior angle between humerous and claw double l_Theta1_1 = 90-elbowAngle/2; - //angle between ground and claw + + // Angle between ground and claw double l_ThetaGroundClaw = ShoulderAngle - l_Theta1_1; // Define values - double l_radius = l_Hypotenus * Math.cos(l_ThetaGroundClaw * Math.PI / 180); - double l_height = l_Hypotenus * Math.sin(l_ThetaGroundClaw * Math.PI / 180); + double l_radius = l_shoulderToClaw * Math.cos(l_ThetaGroundClaw * Math.PI / 180); + double l_height = l_shoulderToClaw * Math.sin(l_ThetaGroundClaw * Math.PI / 180); - //azimuth does not need to be converted - double[] l_currentPos = {azimuthAngle,l_radius,l_height}; + // Azimuth does not need to be converted + double[] l_currentPos = {azimuthAngle, l_radius, l_height}; return l_currentPos; } public double[] getArmPositionXYZ() { // Calculations done in Radians for 2 segment arm of equal segment lengths //Length from shoulder to claw - double l_x,l_y,l_z; + double l_x, l_y, l_z; double l_Hypotenus = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180); //intetior angle between humerous and claw double l_Theta1_1 = 90-elbowAngle/2; diff --git a/subsystems/ArmSubsystem/SparkmaxMotor.java b/subsystems/ArmSubsystem/SparkmaxMotor.java index a853d14..ee13877 100644 --- a/subsystems/ArmSubsystem/SparkmaxMotor.java +++ b/subsystems/ArmSubsystem/SparkmaxMotor.java @@ -1,20 +1,20 @@ package frc.robot.subsystems.ArmSubsystem; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxLimitSwitch; -import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkLimitSwitch; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.MotorType; public class SparkmaxMotor { protected CANSparkMax m_motor; - protected SparkMaxPIDController m_pidControllerPos; + protected SparkPIDController m_pidControllerPos; protected RelativeEncoder m_encoder; protected double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput; protected boolean m_limitSwitchDirection=false; //false reverse true forward - protected SparkMaxLimitSwitch limitSwitch; + protected SparkLimitSwitch limitSwitch; protected SparkmaxMotor(int deviceID,Boolean limitSwitchDirection, double l_max_output, double l_min_output,boolean l_invert) { m_motor = new CANSparkMax(deviceID, MotorType.kBrushless); @@ -22,9 +22,9 @@ protected SparkmaxMotor(int deviceID,Boolean limitSwitchDirection, double l_max_ m_pidControllerPos = m_motor.getPIDController(); m_encoder = m_motor.getEncoder(); if(m_limitSwitchDirection){ - limitSwitch = m_motor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + limitSwitch = m_motor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); }else{ - limitSwitch = m_motor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + limitSwitch = m_motor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); } initializePID(l_max_output,l_min_output); m_motor.setInverted(l_invert); diff --git a/subsystems/DriveSubsystem/DriveControlCommand.java b/subsystems/DriveSubsystem/DriveControlCommand.java new file mode 100644 index 0000000..fcca139 --- /dev/null +++ b/subsystems/DriveSubsystem/DriveControlCommand.java @@ -0,0 +1,66 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.DriveSubsystem; //it is in this package so it can control the arm +//without lowering the restrictions on the private and protected classes within the ArmSubsystem +//If it is not in the same package, then it leaves the Subsystem Vulnerable to access outside +//the command structure + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controllers.PersonalizedController; + +/** An example command that uses an example subsystem. */ +public class DriveControlCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final DriveSubsystem m_driveSubsystem; + private final PersonalizedController m_controller; + //private int i=0; + private static double[] m_FTheta= new double[2]; + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public DriveControlCommand(DriveSubsystem subsystem, PersonalizedController Controller) { + m_driveSubsystem = subsystem; + m_controller=Controller; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_driveSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("drive Enabled"); + getFTheta(() -> (m_controller.ArmInOutAxis()),() -> (m_controller.ArmLeftRightAxis())); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + getFTheta(() -> (m_controller.ArmInOutAxis()),() -> (m_controller.ArmLeftRightAxis())); + m_driveSubsystem.arcadeDriveSquared(m_FTheta[0],m_FTheta[1]); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("drive Disabled"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + private void getFTheta(DoubleSupplier l_fwd,DoubleSupplier l_theta){ + + m_FTheta[0]=l_fwd.getAsDouble(); + m_FTheta[1]=-l_theta.getAsDouble(); + } +} diff --git a/subsystems/DriveSubsystem/DriveSubsystem.java b/subsystems/DriveSubsystem/DriveSubsystem.java new file mode 100644 index 0000000..e32378b --- /dev/null +++ b/subsystems/DriveSubsystem/DriveSubsystem.java @@ -0,0 +1,148 @@ +package frc.robot.subsystems.DriveSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import frc.robot.Constants.DriveConstants; + + +import java.util.function.DoubleSupplier; + + +//Set up all the system objects (motors and encoders) +//note, constants need to be updated as of Feb 20 +public class DriveSubsystem extends SubsystemBase { + private final VictorSP MotorLeft1 = new VictorSP(DriveConstants.MOTORLEFT1PORT); + private final VictorSP MotorLeft2 = new VictorSP(DriveConstants.MOTORLEFT2PORT); + private final VictorSP MotorRight1 = new VictorSP(DriveConstants.MOTORRIGHT1PORT); + private final VictorSP MotorRight2 = new VictorSP(DriveConstants.MOTORRIGHT2PORT); + + private static double[] m_driveFTheta= new double[1]; +//The motors on the right side of the drive. + + + // The left-side drive encoder + private final Encoder EncoderLeft = + new Encoder( + DriveConstants.ENCODERPORTSLEFT[0], + DriveConstants.ENCODERPORTSLEFT[1], + DriveConstants.ENCODERREVERSEDLEFT); + + // The right-side drive encoder + private final Encoder EncoderRight = + new Encoder( + DriveConstants.ENCODERPORTSRIGHT[0], + DriveConstants.ENCODERPORTSRIGHT[1], + DriveConstants.ENCODERREVERSEDRIGHT); + + private final DifferentialDrive DiffDrive = new DifferentialDrive(MotorLeft1::set,MotorRight1::set); + + + public void initialize(){ + //ControlGroupRightMotors.setInverted(true); //must decide if we want to keep this, or switch the wires so that the lights + //on the victor spx motro controllers all flas green when driving forward. + + // Sets the distance per pulse for the encoders + EncoderLeft.setDistancePerPulse(DriveConstants.ENCODERDISTANCEPERPULSEINCHES); + EncoderRight.setDistancePerPulse(DriveConstants.ENCODERDISTANCEPERPULSEINCHES); + MotorLeft1.addFollower(MotorLeft2); + MotorRight1.addFollower(MotorRight2); + } + // simple arcade drive with squared inputs + public void arcadeDriveSquared(Double fwd, Double rot){ + + DiffDrive.arcadeDrive(Math.abs(fwd)*fwd,Math.abs(rot)*rot); + } + + // Untested, this should should reduce the sensitivity of the steering + //based on the real speed of the tracks as read by the encoders + public void arcadeDriveAdaptiveSteering(Double fwd, Double rot){ + double adaptedrot= rot/(1+((EncoderLeft.getRate()+EncoderRight.getRate())*DriveConstants.ADAPTIVESTEERINGSENSITIVITY)); + DiffDrive.arcadeDrive(Math.abs(fwd)*fwd, adaptedrot); + } + + + + // Untested, this should use PID to make it drive straight + //it probably will have some issues as some of the values approach zero + PIDController drivePID = new PIDController(0, 0, 0);//currently set to zero just to test how it reacts without PID + public void driveStraightPID(Double fwd, Double rot){ + double adaptedrot= rot/(1+((EncoderLeft.getRate()+EncoderRight.getRate())*DriveConstants.ADAPTIVESTEERINGSENSITIVITY)); + double rightSpeedTarget= DifferentialDrive.arcadeDriveIK(fwd, adaptedrot, false).right; + double leftSpeedTarget =DifferentialDrive.arcadeDriveIK(fwd, adaptedrot, false).left;;//no rotation + double relativeLeft= (leftSpeedTarget!=0)?EncoderLeft.getRate()/leftSpeedTarget:0; + double relativeRight= (rightSpeedTarget!=0)?EncoderRight.getRate()/rightSpeedTarget:0;//EncoderRight.getRate()/fwd; + double PIDCorrection= drivePID.calculate((relativeLeft-relativeRight),0); + + DiffDrive.tankDrive(leftSpeedTarget+PIDCorrection, rightSpeedTarget-PIDCorrection); + } + + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + +/** + * Returns a command that drives the robot with arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public Command arcadeDriveSquaredCommand(DoubleSupplier fwd, DoubleSupplier rot) { + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + return run(() -> this.arcadeDriveSquared(fwd.getAsDouble(), rot.getAsDouble())) + .withName("arcadeDrive"); + } + + public Command arcadeDriveAdaptiveSteeringCommand(DoubleSupplier fwd, DoubleSupplier rot) { + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + return run(() -> this.arcadeDriveAdaptiveSteering(fwd.getAsDouble(), rot.getAsDouble())) + .withName("arcadeDriveAdaptiveSteering"); + } + + /** + * Returns a command that drives the robot forward a specified distance at a specified speed. + * + * @param distanceMeters The distance to drive forward in meters + * @param speed The fraction of max speed at which to drive + */ + public Command driveDistanceCommand(double distanceMeters, double speed) { + return runOnce( + () -> { + // Reset encoders at the start of the command + EncoderLeft.reset(); + EncoderRight.reset(); + }) + // Drive forward at specified speed + .andThen(run(() -> DiffDrive.arcadeDrive(speed, 0))) + // End command when we've traveled the specified distance + .until( + () -> + Math.max(EncoderLeft.getDistance(), EncoderRight.getDistance()) + >= distanceMeters) + // Stop the drive when the command ends + .finallyDo(interrupted -> DiffDrive.stopMotor()); + } +} diff --git a/subsystems/LimelightSubsystem.java b/subsystems/LimelightSubsystem.java new file mode 100644 index 0000000..64be01f --- /dev/null +++ b/subsystems/LimelightSubsystem.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj2.command.Command; + + + +public class LimelightSubsystem extends SubsystemBase{ + static NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + static NetworkTableEntry Tx = table.getEntry("tx"); + static NetworkTableEntry Ty = table.getEntry("ty"); + static NetworkTableEntry Ta = table.getEntry("ta"); + static NetworkTableEntry Pipeline = table.getEntry("pipeline"); + static NetworkTableEntry Tv = table.getEntry("tv"); //are there any valid targets + + + public double[] getTargets(){ + //Brandon Feb 15: I solved the error you were having with the smartDashboard. + //It needs to be called within a method, not within the construction of the Class. + //you should probably move this into the Subsystems folder too. + + //read values periodically + double x = Tx.getDouble(0.0); + double y = Ty.getDouble(0.0); + double area = Ta.getDouble(0.0); + double[] returnArray= {x,y,area}; + + //post to smart dashboard periodically + SmartDashboard.putNumber("LimelightX", x); + SmartDashboard.putNumber("LimelightY", y); + SmartDashboard.putNumber("LimelightArea", area); + + return returnArray; + } + public void setPipeline(int m_pipeLine){ + Pipeline.setNumber(m_pipeLine); + } + public boolean hasTargets(){ + boolean returnBool =false; + if((double)Tv.getNumber(0)>0){//for some reason tv.getnumber must be mapped to double, not int or boolean + returnBool=true; + } + return returnBool; + } + + + public Command getTargetsCommand() { + return runOnce( ()->getTargets()); + } + public Command checkForTargetsCommand() { + return runOnce( ()->hasTargets()); + } + public Command setPipeline9Command() { + return runOnce( ()->this.setPipeline(9)); + } + + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> setPipeline(7)); + }; + +} \ No newline at end of file