diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 36fafd3..bc8776b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -197,10 +197,10 @@ public final class Constants { public static final int LEFT_CLAW_ID = 44; public static final int RIGHT_CLAW_ID = 45; - public static final double OPEN_POSITION = 0; - public static final double CLOSE_POSITION = 0; + public static final double OPEN_POSITION = 0; // TODO: find actual position + public static final double CLOSE_POSITION = 0; // TODO: find actual position - public static final double THRESHOLD = 0; + public static final double THRESHOLD = 0; // TODO: find actual threshold public static final double CALIBRATION_SPEED = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6290768..2d5a2d4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,7 +57,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; import frc4388.robot.commands.RunClaw; -import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; @@ -92,7 +92,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + private final ClimberRewrite m_robotClimber= new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4044cc9..63f0ac1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -63,7 +64,7 @@ public class RobotMap { public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); + public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); public SwerveModule leftFront; public SwerveModule leftBack; @@ -171,8 +172,10 @@ public class RobotMap { public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO /* Hooks Subsystem */ - public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); - public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless); +// public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); +// public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless); + public final Servo leftClaw = new Servo(1); // TODO: find actual channel + public final Servo rightClaw = new Servo(1); // TODO: find actual channel // Shooter Config /* Boom Boom Subsystem */ diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java index 2cf76af..f3c70ec 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -47,6 +47,7 @@ public class RunClaw extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return m_claws.checkSwitchAndCurrent(clawType); + // return m_claws.checkSwitchAndCurrent(clawType); + return false; // TODO: real return } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index f4ef26f..c4f3f3f 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -7,18 +7,23 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.ClimberConstants; public class Claws extends SubsystemBase { - public CANSparkMax m_leftClaw; - public CANSparkMax m_rightClaw; - private SparkMaxLimitSwitch m_leftLimitSwitchF; - private SparkMaxLimitSwitch m_rightLimitSwitchF; - private SparkMaxLimitSwitch m_leftLimitSwitchR; - private SparkMaxLimitSwitch m_rightLimitSwitchR; + public Servo m_leftClaw; + public Servo m_rightClaw; + + // public CANSparkMax m_leftClaw; + // public CANSparkMax m_rightClaw; + + // private SparkMaxLimitSwitch m_leftLimitSwitchF; + // private SparkMaxLimitSwitch m_rightLimitSwitchF; + // private SparkMaxLimitSwitch m_leftLimitSwitchR; + // private SparkMaxLimitSwitch m_rightLimitSwitchR; private double m_leftOffset; private double m_rightOffset; @@ -26,21 +31,22 @@ public class Claws extends SubsystemBase { private boolean m_open; public static enum ClawType {LEFT, RIGHT} - public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { + public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) { m_leftClaw = leftClaw; m_rightClaw = rightClaw; - m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol - m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol + // m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitchF.enableLimitSwitch(true); - m_rightLimitSwitchF.enableLimitSwitch(true); - m_leftLimitSwitchR.enableLimitSwitch(true); - m_rightLimitSwitchR.enableLimitSwitch(true); - leftClaw.setInverted(true); - rightClaw.setInverted(true); + // m_leftLimitSwitchF.enableLimitSwitch(true); + // m_rightLimitSwitchF.enableLimitSwitch(true); + // m_leftLimitSwitchR.enableLimitSwitch(true); + // m_rightLimitSwitchR.enableLimitSwitch(true); + + // m_leftClaw.setInverted(true); + // m_rightClaw.setInverted(true); m_open = false; @@ -48,11 +54,6 @@ public class Claws extends SubsystemBase { // m_rightClaw.set(ClawConstants.CALIBRATION_SPEED); } - public void setSpeed(double speed){ - m_leftClaw.set(speed); - m_rightClaw.set(speed); - } - /** * Run a specific claw to open or close. * @param which Which claw to run. @@ -66,38 +67,33 @@ public class Claws extends SubsystemBase { // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; // m_leftClaw.getEncoder().setPosition(setPos); - m_leftClaw.set(direction * 0.1); + m_leftClaw.setSpeed(direction * 0.1); } else if (which == Claws.ClawType.RIGHT) { // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; // m_rightClaw.getEncoder().setPosition(setPos); - m_rightClaw.set(direction * 0.1); + m_rightClaw.setSpeed(direction * 0.1); } m_open = open; } - - public void runClaw(ClawType which, double input) { - - if (which == Claws.ClawType.LEFT) { - m_leftClaw.set(input); + m_leftClaw.setSpeed(input); } else if (which == Claws.ClawType.RIGHT) { - m_rightClaw.set(input); + m_rightClaw.setSpeed(input); } } - - public void runClaws(double input) { - m_leftClaw.set(input); - m_rightClaw.set(input); + m_leftClaw.setSpeed(input); + m_rightClaw.setSpeed(input); } + /** * Sets the state of both hooks * @param open The state of the hooks @@ -114,8 +110,6 @@ public class Claws extends SubsystemBase { m_leftClaw.set(-0.1); m_rightClaw.set(-0.1); } - - // m_open = open; } public double[] getOffsets() { @@ -123,12 +117,12 @@ public class Claws extends SubsystemBase { } public boolean fullyOpen() { - return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } + return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && + Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } public boolean fullyClosed() { - return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; + return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && + Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; } /** @@ -137,26 +131,33 @@ public class Claws extends SubsystemBase { * @param limit The current limit. * @return Whether to interrupt the RunClaw command or not. */ - public boolean checkSwitchAndCurrent(ClawType which) { - if (which == ClawType.LEFT) { - if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - return true; - } - } - else if (which == ClawType.RIGHT) { - if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - return true; - } - } - return false; - } + // public boolean checkSwitchAndCurrent(ClawType which) { + // if (which == ClawType.LEFT) { + // if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { + // return true; + // } + // } + // else if (which == ClawType.RIGHT) { + // if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { + // return true; + // } + // } + // return false; + // } @Override public void periodic() { - if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) - m_leftOffset = m_leftClaw.getEncoder().getPosition(); + if (fullyOpen() || fullyClosed()) { + m_leftClaw.setSpeed(0.0); + m_rightClaw.setSpeed(0.0); + } + + // if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) + // // m_leftOffset = m_leftClaw.getEncoder().getPosition(); + // m_leftOffset = m_leftClaw.getPosition(); - if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) - m_rightOffset = m_rightClaw.getEncoder().getPosition(); + // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) + // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); + // m_rightOffset = m_rightClaw.getPosition(); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 42b5886..cb8a9f1 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -1,279 +1,279 @@ -// 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. +// // 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. -/* -Safety -Hooks -Add -*/ +// /* +// Safety +// Hooks +// Add +// */ -package frc4388.robot.subsystems; +// package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +// import com.ctre.phoenix.motorcontrol.NeutralMode; +// import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import com.ctre.phoenix.sensors.WPI_PigeonIMU; -import org.opencv.core.Point; +// import org.opencv.core.Point; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; -import frc4388.robot.Constants.ClimberConstants; +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc4388.robot.Constants; +// import frc4388.robot.Constants.ClimberConstants; -public class Climber extends SubsystemBase { - public WPI_TalonFX m_shoulder; - public WPI_TalonFX m_elbow; +// public class Climber extends SubsystemBase { +// public WPI_TalonFX m_shoulder; +// public WPI_TalonFX m_elbow; - private double m_shoulderOffset; - private double m_elbowOffset; +// private double m_shoulderOffset; +// private double m_elbowOffset; - private WPI_PigeonIMU m_gyro; - private boolean m_groundRelative; - private double m_robotAngle; - private double m_robotPosition; +// private WPI_PigeonIMU m_gyro; +// private boolean m_groundRelative; +// private double m_robotAngle; +// private double m_robotPosition; - private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d); +// private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d); - public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { - m_shoulder = shoulder; - m_shoulder.configFactoryDefault(); - m_shoulder.setNeutralMode(NeutralMode.Brake); +// public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { +// m_shoulder = shoulder; +// m_shoulder.configFactoryDefault(); +// m_shoulder.setNeutralMode(NeutralMode.Brake); - m_elbow = elbow; - m_elbow.configFactoryDefault(); - m_elbow.setNeutralMode(NeutralMode.Brake); +// m_elbow = elbow; +// m_elbow.configFactoryDefault(); +// m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberGains(); +// setClimberGains(); - m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); - m_elbowOffset = m_elbow.getSelectedSensorPosition(); +// m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); +// m_elbowOffset = m_elbow.getSelectedSensorPosition(); - m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); - m_elbow.configForwardSoftLimitEnable(false); - m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - m_elbow.configReverseSoftLimitEnable(false); +// m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); +// m_elbow.configForwardSoftLimitEnable(false); +// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); +// m_elbow.configReverseSoftLimitEnable(false); - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); - m_shoulder.configForwardSoftLimitEnable(false); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); - m_shoulder.configReverseSoftLimitEnable(false); +// m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); +// m_shoulder.configForwardSoftLimitEnable(false); +// m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); +// m_shoulder.configReverseSoftLimitEnable(false); - if(groundRelative) - m_gyro = gyro; +// if(groundRelative) +// m_gyro = gyro; - m_groundRelative = groundRelative; - } +// m_groundRelative = groundRelative; +// } - /* getJointAngles - * Gets joint angles for climber arm - * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the - * rotation of the robot. Both parameters should be in cm. - * returns [shoulderAngle, elbowAngle] in radians - * Does not set the motors automatically - * - * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ - public static double[] getJointAngles(Point targetPoint, double tiltAngle) { - double [] angles = new double[2]; +// /* getJointAngles +// * Gets joint angles for climber arm +// * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the +// * rotation of the robot. Both parameters should be in cm. +// * returns [shoulderAngle, elbowAngle] in radians +// * Does not set the motors automatically +// * +// * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ +// public static double[] getJointAngles(Point targetPoint, double tiltAngle) { +// double [] angles = new double[2]; - double mag = Math.hypot(targetPoint.x, targetPoint.y); - if(mag >= ClimberConstants.MAX_ARM_LENGTH) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; - mag = ClimberConstants.MAX_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; - mag = ClimberConstants.MIN_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { - targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = 0.d; - mag = ClimberConstants.MIN_ARM_LENGTH; - } +// double mag = Math.hypot(targetPoint.x, targetPoint.y); +// if(mag >= ClimberConstants.MAX_ARM_LENGTH) { +// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; +// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; +// mag = ClimberConstants.MAX_ARM_LENGTH; +// } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { +// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; +// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; +// mag = ClimberConstants.MIN_ARM_LENGTH; +// } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { +// targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; +// targetPoint.y = 0.d; +// mag = ClimberConstants.MIN_ARM_LENGTH; +// } - // The angle to the target point - double theta; - if(targetPoint.x == 0.d) { - theta = Math.PI/2.d; // TODO rename variable - } else { - theta = Math.atan(targetPoint.y / targetPoint.x); - } - theta += tiltAngle; +// // The angle to the target point +// double theta; +// if(targetPoint.x == 0.d) { +// theta = Math.PI/2.d; // TODO rename variable +// } else { +// theta = Math.atan(targetPoint.y / targetPoint.x); +// } +// theta += tiltAngle; - if(targetPoint.x < 0.d) - theta += Math.PI; +// if(targetPoint.x < 0.d) +// theta += Math.PI; - // Correct target position for tilt - targetPoint.x = Math.cos(theta) * mag; - targetPoint.y = Math.sin(theta) * mag; +// // Correct target position for tilt +// targetPoint.x = Math.cos(theta) * mag; +// targetPoint.y = Math.sin(theta) * mag; - // Law and Order: Cosines edition - double shoulderAngle; - if(mag == 0) { - shoulderAngle = 0; - } else { - shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); - } - shoulderAngle = theta - shoulderAngle; +// // Law and Order: Cosines edition +// double shoulderAngle; +// if(mag == 0) { +// shoulderAngle = 0; +// } else { +// shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / +// (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); +// } +// shoulderAngle = theta - shoulderAngle; - double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); - //elbowAngle = Math.PI - elbowAngle; - // System.out.println("sa1: " + shoulderAngle); - // System.out.println("ea1: " + elbowAngle); +// double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / +// (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); +// //elbowAngle = Math.PI - elbowAngle; +// // System.out.println("sa1: " + shoulderAngle); +// // System.out.println("ea1: " + elbowAngle); - // If the climber is resting on the robot base, rotate the upper arm in the direction of the target - if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { - shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; - double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; - // System.out.println("xDiff: " + xDiff); +// // If the climber is resting on the robot base, rotate the upper arm in the direction of the target +// if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { +// shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; +// double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; +// // System.out.println("xDiff: " + xDiff); - elbowAngle = Math.atan(-targetPoint.y / xDiff); - // System.out.println("ea2: " + elbowAngle); - if(elbowAngle < 0) { - elbowAngle = Math.PI - Math.abs(elbowAngle); - } +// elbowAngle = Math.atan(-targetPoint.y / xDiff); +// // System.out.println("ea2: " + elbowAngle); +// if(elbowAngle < 0) { +// elbowAngle = Math.PI - Math.abs(elbowAngle); +// } - if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) - elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; - // System.out.println("ea3: " + elbowAngle); +// if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) +// elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; +// // System.out.println("ea3: " + elbowAngle); - // Deal with negative wraparound - // if(xDiff >= 0.d) - // elbowAngle += Math.PI; - // System.out.println("ea4: " + elbowAngle); - } +// // Deal with negative wraparound +// // if(xDiff >= 0.d) +// // elbowAngle += Math.PI; +// // System.out.println("ea4: " + elbowAngle); +// } - angles[0] = shoulderAngle; - angles[1] = elbowAngle; - return angles; - } +// angles[0] = shoulderAngle; +// angles[1] = elbowAngle; +// return angles; +// } -public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput); - m_elbow.set(elbowOutput); -} +// public void setMotors(double shoulderOutput, double elbowOutput) { +// m_shoulder.set(shoulderOutput); +// m_elbow.set(elbowOutput); +// } -/* Rotation Matrix - R = [cos(0) -sin(0) \n sin(0) cos(0)] - Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)] - Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix - Rotation Matrix video: https://youtu.be/Ta8cKqltPfU - */ - public double getRobotTilt() { - double[] ypr = new double[3]; - m_gyro.getYawPitchRoll(ypr); - double theta = 0; +// /* Rotation Matrix +// R = [cos(0) -sin(0) \n sin(0) cos(0)] +// Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)] +// Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix +// Rotation Matrix video: https://youtu.be/Ta8cKqltPfU +// */ +// public double getRobotTilt() { +// double[] ypr = new double[3]; +// m_gyro.getYawPitchRoll(ypr); +// double theta = 0; - // double sin; - // double cos; - // xsin = 0; //placeholder for sin, cos - // ysin = 0; - // xcos = 0; - // ycos = 0; - double[][] rotMax = { - {Math.cos(theta) - Math.sin(theta), 0 }, - {Math.sin(theta) + Math.cos(theta), 0}, - {0, 0, 1} - }; +// // double sin; +// // double cos; +// // xsin = 0; //placeholder for sin, cos +// // ysin = 0; +// // xcos = 0; +// // ycos = 0; +// double[][] rotMax = { +// {Math.cos(theta) - Math.sin(theta), 0 }, +// {Math.sin(theta) + Math.cos(theta), 0}, +// {0, 0, 1} +// }; - if (m_robotPosition != m_robotAngle){ - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); - } +// if (m_robotPosition != m_robotAngle){ +// setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); +// } - return Math.toRadians(ypr[1]); // Pitch - // multiply by pie and then divide by 180 +// return Math.toRadians(ypr[1]); // Pitch +// // multiply by pie and then divide by 180 - } +// } - public void setClimberGains() { - // shoulder PIDs - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); +// public void setClimberGains() { +// // shoulder PIDs +// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); +// m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - // elbow PIDs - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } +// // elbow PIDs +// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); +// m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); +// m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); +// } - public void setJointAngles(double[] angles) { - System.out.println(angles); - setJointAngles(angles[0], angles[1]); - } +// public void setJointAngles(double[] angles) { +// System.out.println(angles); +// setJointAngles(angles[0], angles[1]); +// } - public void setJointAngles(double shoulderAngle, double elbowAngle) { - shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; - elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; +// public void setJointAngles(double shoulderAngle, double elbowAngle) { +// shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; +// elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; - shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; - elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; +// shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; +// elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; - // Convert radians to ticks - System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); +// // Convert radians to ticks +// System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); - double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; +// double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; +// double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; - elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; +// shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; +// elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; - shoulderPosition += m_shoulderOffset; - elbowPosition += m_elbowOffset; +// shoulderPosition += m_shoulderOffset; +// elbowPosition += m_elbowOffset; - m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); - m_elbow.set(TalonFXControlMode.Position, elbowPosition); - } +// m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); +// m_elbow.set(TalonFXControlMode.Position, elbowPosition); +// } - public void controlWithInput(double xInput, double yInput) { - m_position.x += xInput * ClimberConstants.MOVE_SPEED; - m_position.y += yInput * ClimberConstants.MOVE_SPEED; +// public void controlWithInput(double xInput, double yInput) { +// m_position.x += xInput * ClimberConstants.MOVE_SPEED; +// m_position.y += yInput * ClimberConstants.MOVE_SPEED; - System.out.println("x: " + m_position.x + " y: " + m_position.y); +// System.out.println("x: " + m_position.x + " y: " + m_position.y); - double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; +// double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; - // double[] testAngles = getJointAngles(0, 0, 0); - // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); +// // double[] testAngles = getJointAngles(0, 0, 0); +// // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); - // double[] testAngles2 = getJointAngles(5000, 5000, 0); - // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); +// // double[] testAngles2 = getJointAngles(5000, 5000, 0); +// // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); - // double[] testAngles3 = getJointAngles(0, 75, 0); - // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); +// // double[] testAngles3 = getJointAngles(0, 75, 0); +// // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); - // double[] testAngles4 = getJointAngles(75, 0, 0); - // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); +// // double[] testAngles4 = getJointAngles(75, 0, 0); +// // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); - // double[] testAngles5 = getJointAngles(-75, 0, 0); - // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); +// // double[] testAngles5 = getJointAngles(-75, 0, 0); +// // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); - // double[] testAngles6 = getJointAngles(60, 25, 0); - // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); +// // double[] testAngles6 = getJointAngles(60, 25, 0); +// // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); - double[] jointAngles = getJointAngles(m_position, tiltAngle); - setJointAngles(jointAngles); - } +// double[] jointAngles = getJointAngles(m_position, tiltAngle); +// setJointAngles(jointAngles); +// } - public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { - m_robotPosition = robotPosition; - m_robotAngle = robotAngle; - m_robotAngle = 45; //45 is placeholder - } +// public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { +// m_robotPosition = robotPosition; +// m_robotAngle = robotAngle; +// m_robotAngle = 45; //45 is placeholder +// } - @Override - public void periodic(){ - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - } -} +// @Override +// public void periodic(){ +// SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); +// SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); +// } +// }