diff --git a/src/main/java/com/team766/framework/AutonomousMode.java b/src/main/java/com/team766/framework/AutonomousMode.java index d29337be..a7159049 100644 --- a/src/main/java/com/team766/framework/AutonomousMode.java +++ b/src/main/java/com/team766/framework/AutonomousMode.java @@ -11,6 +11,8 @@ public AutonomousMode(String name, Supplier constructor) { m_name = name; } + + public Procedure instantiate() { return m_constructor.get(); } diff --git a/src/main/java/com/team766/hal/GenericRobotMain.java b/src/main/java/com/team766/hal/GenericRobotMain.java index e4b72e15..4cb900c7 100755 --- a/src/main/java/com/team766/hal/GenericRobotMain.java +++ b/src/main/java/com/team766/hal/GenericRobotMain.java @@ -98,8 +98,8 @@ public void autonomousInit() { m_oiContext = null; } - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); + //Robot.gyro.resetGyro180(); + //Robot.drive.setGyro(Robot.gyro.getGyroYaw()); if (m_autonomous != null) { Logger.get(Category.AUTONOMOUS).logRaw(Severity.INFO, "Continuing previous autonomus procedure " + m_autonomous.getContextName()); diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java index 26f1b134..16f9890b 100644 --- a/src/main/java/com/team766/odometry/Odometry.java +++ b/src/main/java/com/team766/odometry/Odometry.java @@ -198,4 +198,4 @@ public PointDir run() { } return currentPosition; } -} +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/AutonomousModes.java b/src/main/java/com/team766/robot/AutonomousModes.java index fb9085a4..7665c660 100644 --- a/src/main/java/com/team766/robot/AutonomousModes.java +++ b/src/main/java/com/team766/robot/AutonomousModes.java @@ -21,11 +21,11 @@ public class AutonomousModes { // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), // new AutonomousMode("FollowPoints", () -> new FollowPoints()), // new AutonomousMode("ReverseIntake", () -> new ReverseIntake()), - new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), + //new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()), // new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()), // new AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()), // new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")), // //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})), - // new AutonomousMode("DoNothing", () -> new DoNothing()), + //new AutonomousMode("DoNothing", () -> new DoNothing()), }; } \ No newline at end of file diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 871d7174..98fb8bcd 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -1,6 +1,7 @@ package com.team766.robot; import com.team766.framework.Procedure; +import org.photonvision.targeting.PhotonTrackedTarget; import com.team766.framework.Context; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; @@ -8,9 +9,10 @@ import com.team766.robot.constants.InputConstants; import com.team766.robot.constants.InputConstants.IntakeState; import com.team766.robot.procedures.*; -import com.team766.robot.mechanisms.Drive; +import com.team766.robot.mechanisms.*; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,6 +22,40 @@ */ public class OI extends Procedure { + private JoystickReader joystickOne; + public OI(){ + joystickOne =RobotProvider.instance.getJoystick(0); + } + + public void run(Context context){ + context.takeOwnership(Robot.JanuaryTag); + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.intake); + + Transform3d btt = Robot.JanuaryTag.getBestTag(); + PhotonTrackedTarget t = Robot.JanuaryTag.getBestTrackedTarget(); + + if(joystickOne.getButton(1)){ + int output = Robot.drive.PhotonDrive(btt, t); + + if(output == 1){ + Robot.intake.reverseIntake(); + Robot.storage.beltOut(); + }else{ + Robot.intake.stopIntake(); + Robot.storage.beltIdle(); + } + + + } + + // if(joystickOne.getButtonReleased(1)){ + // Robot.drive.drive2D(0,0); + // } + + + } +/* private JoystickReader leftJoystick; private JoystickReader rightJoystick; private JoystickReader controlPanel; @@ -57,81 +93,89 @@ public class OI extends Procedure { public OI() { loggerCategory = Category.OPERATOR_INTERFACE; - - leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); - rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); - controlPanel = RobotProvider.instance.getJoystick(InputConstants.CONTROL_PANEL); + j1 = RobotProvider.instance.getJoystick(0); + //leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK); + //rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK); + //controlPanel = RobotProvider.instance.getJoystick(InputConstants.CONTROL_PANEL); } public void run(Context context) { + context.takeOwnership(Robot.JT); context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.arms); - context.takeOwnership(Robot.storage); - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.grabber); + + + Robot.JT.swerveCalculate(); + if(j1.getButton(1)){ + Robot.drive.drive2D(Robot.JT.getXSpeed(), Robot.JT.getYSpeed()); + } + + // context.takeOwnership(Robot.intake); + // context.takeOwnership(Robot.arms); + // context.takeOwnership(Robot.storage); + // context.takeOwnership(Robot.gyro); + // context.takeOwnership(Robot.grabber); - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); - while (true) { - context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); - RobotProvider.instance.refreshDriverStationData(); + // while (true) { + // context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); + // RobotProvider.instance.refreshDriverStationData(); - // Add driver controls here - make sure to take/release ownership - // of mechanisms when appropriate. + // // Add driver controls here - make sure to take/release ownership + // // of mechanisms when appropriate. - leftJoystickX = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); - leftJoystickY = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)); - rightJoystickX = Drive.correctedJoysticks(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); + // leftJoystickX = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); + // leftJoystickY = Drive.correctedJoysticks(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)); + // rightJoystickX = Drive.correctedJoysticks(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)); + // Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - // if (DriverStation.getAlliance() == Alliance.Red) { - // SmartDashboard.putString("Alliance", "RED"); - // } else if (DriverStation.getAlliance() == Alliance.Blue) { - // SmartDashboard.putString("Alliance", "BLUE"); - // } else { - // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); - // } + // // if (DriverStation.getAlliance() == Alliance.Red) { + // // SmartDashboard.putString("Alliance", "RED"); + // // } else if (DriverStation.getAlliance() == Alliance.Blue) { + // // SmartDashboard.putString("Alliance", "BLUE"); + // // } else { + // // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); + // // } - if (controlPanel.getButtonPressed(InputConstants.RESET_GYRO)) { - Robot.gyro.resetGyro(); - } + // if (controlPanel.getButtonPressed(InputConstants.RESET_GYRO)) { + // Robot.gyro.resetGyro(); + // } - if (controlPanel.getButtonPressed(InputConstants.RESET_POS)) { - Robot.drive.resetCurrentPosition(); - } + // if (controlPanel.getButtonPressed(InputConstants.RESET_POS)) { + // Robot.drive.resetCurrentPosition(); + // } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { - RightJoystick_Y = rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - } else { - RightJoystick_Y = 0; - } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)/2; - } else { - rightJoystickX = 0; - } - if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { - RightJoystick_Theta = rightJoystick.getAxis(InputConstants.AXIS_TWIST); - } else { - RightJoystick_Theta = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { - leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); - } else { - leftJoystickY = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { - leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); - } else { - leftJoystickX = 0; - } - if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { - LeftJoystick_Theta = leftJoystick.getAxis(InputConstants.AXIS_TWIST); - } else { - LeftJoystick_Theta = 0; - } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + // RightJoystick_Y = rightJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + // } else { + // RightJoystick_Y = 0; + // } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + // rightJoystickX = rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)/2; + // } else { + // rightJoystickX = 0; + // } + // if (Math.abs(rightJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { + // RightJoystick_Theta = rightJoystick.getAxis(InputConstants.AXIS_TWIST); + // } else { + // RightJoystick_Theta = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05) { + // leftJoystickY = leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD); + // } else { + // leftJoystickY = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05) { + // leftJoystickX = leftJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT); + // } else { + // leftJoystickX = 0; + // } + // if (Math.abs(leftJoystick.getAxis(InputConstants.AXIS_TWIST)) > 0.05) { + // LeftJoystick_Theta = leftJoystick.getAxis(InputConstants.AXIS_TWIST); + // } else { + // LeftJoystick_Theta = 0; + // } // Sets intake state based on button pressed if (controlPanel.getButtonPressed(InputConstants.INTAKE)){ @@ -156,6 +200,8 @@ public void run(Context context) { intakeState = IntakeState.IDLE; } } */ + + /* if (controlPanel.getButtonPressed(InputConstants.OUTTAKE)){ if (intakeState == IntakeState.IDLE){ Robot.intake.reverseIntake(); @@ -166,88 +212,141 @@ public void run(Context context) { Robot.storage.beltIdle(); intakeState = IntakeState.IDLE; } - } + } */ - // Sets the wheels to the cross position if the cross button is pressed - if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { - if (!isCross) { - context.startAsync(new setCross()); - } - isCross = !isCross; - } + // // Sets the wheels to the cross position if the cross button is pressed + // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + // if (!isCross) { + // context.startAsync(new setCross()); + // } + // isCross = !isCross; + // } - SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + // SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); - // Moves the robot if there are joystick inputs - if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { - context.takeOwnership(Robot.drive); - // If a button is pressed, drive is just fine adjustment - if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { - Robot.drive.swerveDrive((leftJoystickX * FINE_DRIVING_COEFFICIENT), (-leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); - } else { - Robot.drive.swerveDrive((leftJoystickX), (-leftJoystickY), (rightJoystickX)); - } - } else if (!isCross) { - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - } + // // Moves the robot if there are joystick inputs + // if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) { + // context.takeOwnership(Robot.drive); + // // If a button is pressed, drive is just fine adjustment + // if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { + // Robot.drive.swerveDrive((leftJoystickX * FINE_DRIVING_COEFFICIENT), (-leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); + // } else { + // Robot.drive.swerveDrive((leftJoystickX), (-leftJoystickY), (rightJoystickX)); + // } + // } else if (!isCross) { + // Robot.drive.stopDriveMotors(); + // Robot.drive.stopSteerMotors(); + // } + + // // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { + // // context.startAsync(new setCross()); + // // } + + // if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { + // Robot.arms.pidForArmOne(-17.379); + // Robot.arms.pidForArmTwo(-66.61); + // } + // if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { + // Robot.arms.pidForArmOne(3.7765); + // Robot.arms.pidForArmTwo(-97.703); + // } + // if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-90); + // } + // if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-76.964); + // } + // if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-152.387); + // } + // /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-73.664); + // } */ + // if(controlPanel.getButton(InputConstants.NUDGE_UP)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); + // } + + // if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); + // } + // Robot.arms.logs(); + + // if(controlPanel.getButton(InputConstants.GRAB_IN)){ + // Robot.grabber.grabberPickUp(); + // } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ + // Robot.grabber.grabberLetGo(); + // } else { + // Robot.grabber.grabberStop(); + // } + + // if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { + // Robot.arms.brake(); + // } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { + // Robot.arms.coast(); + // } + + // if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ + // Robot.arms.armStop(); + // } // if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) { // context.startAsync(new setCross()); // } - if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { - Robot.arms.pidForArmOne(-17.379); - Robot.arms.pidForArmTwo(-66.61); - } - if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { - Robot.arms.pidForArmOne(3.7765); - Robot.arms.pidForArmTwo(-97.703); - } - if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { - Robot.arms.pidForArmOne(17.269); - Robot.arms.pidForArmTwo(-90); - } - if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { - Robot.arms.pidForArmOne(22.73); - Robot.arms.pidForArmTwo(-76.964); - } - if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { - Robot.arms.pidForArmOne(17.269); - Robot.arms.pidForArmTwo(-152.387); - } - /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ - Robot.arms.pidForArmOne(22.73); - Robot.arms.pidForArmTwo(-73.664); - } */ - if(controlPanel.getButton(InputConstants.NUDGE_UP)){ - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); - } + // if(controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { + // Robot.arms.pidForArmOne(-17.379); + // Robot.arms.pidForArmTwo(-66.61); + // } + // if(controlPanel.getButtonPressed(InputConstants.CONE_MID)) { + // Robot.arms.pidForArmOne(3.7765); + // Robot.arms.pidForArmTwo(-97.703); + // } + // if(controlPanel.getButtonPressed(InputConstants.ARM_READY)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-90); + // } + // if(controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-76.964); + // } + // if(controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { + // Robot.arms.pidForArmOne(17.269); + // Robot.arms.pidForArmTwo(-152.387); + // } + // /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ + // Robot.arms.pidForArmOne(22.73); + // Robot.arms.pidForArmTwo(-73.664); + // } */ + // if(controlPanel.getButton(InputConstants.NUDGE_UP)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); + // } - if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); - } - Robot.arms.logs(); - - if(controlPanel.getButton(InputConstants.GRAB_IN)){ - Robot.grabber.grabberPickUp(); - } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ - Robot.grabber.grabberLetGo(); - } else { - Robot.grabber.grabberStop(); - } - - if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { - Robot.arms.brake(); - } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { - Robot.arms.coast(); - } + // if(controlPanel.getButton(InputConstants.NUDGE_DOWN)){ + // Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); + // } + // Robot.arms.logs(); - if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ - Robot.arms.armStop(); - } + // if(controlPanel.getButton(InputConstants.GRAB_IN)){ + // Robot.grabber.grabberPickUp(); + // } else if(controlPanel.getButton(InputConstants.GRAB_OUT)){ + // Robot.grabber.grabberLetGo(); + // } else { + // Robot.grabber.grabberStop(); + // } + + // if (controlPanel.getButtonPressed(InputConstants.BRAKE)) { + // Robot.arms.brake(); + // } else if (controlPanel.getButtonPressed(InputConstants.COAST)) { + // Robot.arms.coast(); + // } + + // if(leftJoystick.getButtonPressed(InputConstants.ARM_STOP)){ + // Robot.arms.armStop(); + // } } - } -} diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index a9bf6946..02821c09 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -3,21 +3,22 @@ import com.team766.robot.mechanisms.*; public class Robot { - // Declare mechanisms here - public static Intake intake; - public static Storage storage; + //Declare mechanisms here + // public static Intake intake; + // public static Storage storage; public static Drive drive; - public static Arms arms; + // public static Arms arms; public static Gyro gyro; - public static Grabber grabber; + // public static Grabber grabber; + public static januaryTag JanuaryTag; public static void robotInit() { - // Initialize mechanisms here - intake = new Intake(); - storage = new Storage(); + //Initialize mechanisms here + // intake = new Intake(); + // storage = new Storage(); drive = new Drive(); - arms = new Arms(); + // arms = new Arms(); gyro = new Gyro(); - grabber = new Grabber(); + //grabber = new Grabber(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java deleted file mode 100644 index 63a09cd9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ /dev/null @@ -1,411 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.revrobotics.CANSparkMax; -import com.team766.config.ConfigFileReader; -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.logging.Category; -//This is for the motor that controls the pulley - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Arms extends Mechanism { - /* - * This defines the motors and casts them to CanSparkMaxs (CSMs) so we can use REV Robotics PID SmartMotion. - * Next, it also defines the absolute encoder. - */ - private MotorController firstJoint = RobotProvider.instance.getMotor("arms.firstJoint"); - private CANSparkMax firstJointCANSparkMax = (CANSparkMax)firstJoint; - private SparkMaxPIDController firstJointPIDController = firstJointCANSparkMax.getPIDController(); - private SparkMaxAbsoluteEncoder altEncoder1 = firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle); - - private MotorController secondJoint = RobotProvider.instance.getMotor("arms.secondJoint"); - private CANSparkMax secondJointCANSparkMax = (CANSparkMax)secondJoint; - private SparkMaxPIDController secondJointPIDController = secondJointCANSparkMax.getPIDController(); - private SparkMaxAbsoluteEncoder altEncoder2 = secondJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle); - - // Non-motor constants - // This is the deadzone, so that the arm(s) don't oscillate. For example, a value of 5 means a 5 relitive encoder unit deadzone in each direction. - private static final double doubleDeadZone = 2; - - // We want firstJoint/secondJoint being straight-up to be 0 rel encoder units - // and counter-clockwise to be positive. - // All the following variables are in degrees - private double firstJointPosition = 0; - private double secondJointPosition = 0; - private double firstJointCombo = 0; - private double secondJointCombo = 0; - - // This sets the maximum locations so we can use them in code to make sure the arm joints dont go past there. - private static final double FIRST_JOINT_MAX_LOCATION = 27.3; - private static final double FIRST_JOINT_MIN_LOCATION = -45; - private static final double SECOND_JOINT_MAX_LOCATION = 45; - private static final double SECOND_JOINT_MIN_LOCATION = -160; - - private RateLimiter runRateLimiter = new RateLimiter(0.05); - - enum ArmState { - PID, - ANTIGRAV, - OFF - } - - boolean jointOneCanContinue = false; - - ArmState theStateOf1 = ArmState.OFF; - ArmState theStateOf2 = ArmState.OFF; - - - private ArmsAntiGrav antiGrav; - - public Arms() { - loggerCategory = Category.MECHANISMS; - - firstJoint.setNeutralMode(NeutralMode.Brake); - secondJoint.setNeutralMode(NeutralMode.Brake); - - // PID Constants - ValueProvider firstJointP = ConfigFileReader.getInstance().getDouble("arms.firstJointP"); - ValueProvider firstJointI = ConfigFileReader.getInstance().getDouble("arms.firstJointI"); - ValueProvider firstJointD = ConfigFileReader.getInstance().getDouble("arms.firstJointD"); - ValueProvider firstJointFF = ConfigFileReader.getInstance().getDouble("arms.firstJointFF"); - - firstJointPIDController.setP(firstJointP.valueOr(0.0006)); - firstJointPIDController.setI(firstJointI.valueOr(0.0)); - firstJointPIDController.setD(firstJointD.valueOr(0.0)); - // FF was 0.002 - firstJointPIDController.setFF(firstJointFF.valueOr(0.001)); - // firstJointPIDController.setSmartMotionAllowedClosedLoopError(3, 0); - - // More PID constants - ValueProvider secondJointP = ConfigFileReader.getInstance().getDouble("arms.secondJointP"); - ValueProvider secondJointI = ConfigFileReader.getInstance().getDouble("arms.secondJointI"); - ValueProvider secondJointD = ConfigFileReader.getInstance().getDouble("arms.secondJointD"); - ValueProvider secondJointFF = ConfigFileReader.getInstance().getDouble("arms.secondJointFF"); - - // TODO: use secondJointO/I/D/FF.valueOr - secondJointPIDController.setP(0.0005); - secondJointPIDController.setI(secondJointI.valueOr(0.0)); - // D was 0.00001 - secondJointPIDController.setD(0.0000); - // FF was 0.00109 - secondJointPIDController.setFF(0.0008); - - // These next things deal a lot with the PID SmartMotion - firstJointCANSparkMax.setInverted(false); - // TODO : consider decrease velocity instead of decreasing power - firstJointPIDController.setSmartMotionMaxVelocity(4000, 0); - firstJointPIDController.setSmartMotionMinOutputVelocity(0, 0); - firstJointPIDController.setSmartMotionMaxAccel(3000, 0); - // firstJointPIDController.setOutputRange(-0.75, 0.75); - firstJointPIDController.setOutputRange(-0.65, 0.65); - firstJointCANSparkMax.setSmartCurrentLimit(40); - // Do not use setSmartMotionAllowedClosedLoopError(5, 0) unless it is safe to test without destorying anything - - // These too - secondJointPIDController.setSmartMotionMaxVelocity(4000, 0); - secondJointPIDController.setSmartMotionMinOutputVelocity(0, 0); - secondJointPIDController.setSmartMotionMaxAccel(3000, 0); - secondJointPIDController.setOutputRange(-1, 1); - secondJointCANSparkMax.setSmartCurrentLimit(40); - - // This resets the degrees and stuff so that we dont have to have the arm at certain positions to reset... - firstJointPIDController.setFeedbackDevice(firstJointCANSparkMax.getEncoder()); - secondJointPIDController.setFeedbackDevice(secondJointCANSparkMax.getEncoder()); - - // absolute encoder zero offsets are set to positions which will never be used - // to avoid wrap-around errors - // first joint zero is horizontal parallel to ground - // second joint zero is when the relative angle is 0 degrees from the first arm segment - altEncoder1.setZeroOffset(0.655); // TODO: these need tweaking from altEncoder1Offset - altEncoder2.setZeroOffset(0.627); - SmartDashboard.putNumber("Alt Encoder 1", altEncoder1.getPosition()); - SmartDashboard.putNumber("Alt Encoder 2", altEncoder2.getPosition()); - - antiGrav = new ArmsAntiGrav(firstJoint, secondJoint); - - // We only want to resetEncoders after configs are loaded, offsets are set, etc - resetFirstEncoders(); - resetSecondEncoders(); - } - - // This allows the pulley motor power to be changed, usually manually - // The magnitude ranges from 0.0-1.0, and sign (positive/negative) determines the direction - - // manual changing of arm 1 - public void manuallySetArmOnePower(double power) { - checkContextOwnership(); - firstJoint.set(power); - } - - // manual changing of arm 2. - public void manuallySetArmTwoPower(double power) { - checkContextOwnership(); - secondJoint.set(power); - } - - public void coast(){ - firstJoint.setNeutralMode(NeutralMode.Coast); - secondJoint.setNeutralMode(NeutralMode.Coast); - } - - public void brake(){ - firstJoint.setNeutralMode(NeutralMode.Brake); - secondJoint.setNeutralMode(NeutralMode.Brake); - } - public double nudgeArm2up(){ - return (ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) +1); - } - - public double nudgeArm2down(){ - return (ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) -1); - } - // Resets encoders - public void resetFirstEncoders() { - checkContextOwnership(); - - // TODO: this offset is to factor in the difference between the - // "zero" for alt encoder and the "zero" when we use degrees - // Offset tuning should be done above in `altEncoder1.setZeroOffset` - final double altEncoder1Offset = 0.25; - // final double altEncoder1Offset = 0.22calc5; - final double altEncoder2Offset = 0.5; - //final double altEncoder2Offset = 0.493; - - // altEncoder1Offset = what is the value of altEncoder1 when firstJoint is vertical - double firstJointAbsEncoder = altEncoder1.getPosition(); - double firstJointRelEncoder = ArmsUtil.AbsToEU(firstJointAbsEncoder - altEncoder1Offset); - - // set the sensor positions and setpoint of our rel encoders - firstJoint.setSensorPosition(firstJointRelEncoder); - firstJointPosition = ArmsUtil.EUTodegrees(firstJointRelEncoder); - } - - public void resetSecondEncoders() { - checkContextOwnership(); - - // TODO: this offset is to factor in the difference between the - // "zero" for alt encoder and the "zero" when we use degrees - // Offset tuning should be done above in `altEncoder1.setZeroOffset` - final double altEncoder1Offset = 0.25; - // final double altEncoder1Offset = 0.22calc5; - final double altEncoder2Offset = 0.5; - //final double altEncoder2Offset = 0.493; - - // altEncoder1Offset = what is the value of altEncoder1 when firstJoint is vertical - // altEncoder2Offset = what is the value of altEncoder2 when secondJoint is colinear w/firstJoint - - double firstJointAbsEncoder = altEncoder1.getPosition(); - double secondJointAbsEncoder = altEncoder2.getPosition(); - double secondJointRelEncoder = ArmsUtil.AbsToEU( - firstJointAbsEncoder - altEncoder1Offset - + secondJointAbsEncoder - altEncoder2Offset); - - // set the sensor positions and setpoint of our rel encoders - secondJoint.setSensorPosition(secondJointRelEncoder); - secondJointPosition = ArmsUtil.EUTodegrees(secondJointRelEncoder); - } - - public void armStop(){ - theStateOf1 = ArmState.OFF; - theStateOf2 = ArmState.OFF; - } - - // for first arm - /** - * Set PID for the first joint. - * - * @param value desired position in degrees. - */ - public void pidForArmOne(double value) { - // This will be run once - // log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - value = ArmsUtil.clampValueToRange(value, FIRST_JOINT_MIN_LOCATION, FIRST_JOINT_MAX_LOCATION); - - firstJointPosition = value; - // if(Math.abs(EUTodegrees(firstJoint.getSensorPosition() ))) - firstJointPIDController.setReference(ArmsUtil.degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getFirstJointPower()); - theStateOf1 = ArmState.PID; - firstJointCombo = 0; - - } - - // PID for second arm - public void pidForArmTwo(double value) { - // log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - - value = ArmsUtil.clampValueToRange(value, SECOND_JOINT_MIN_LOCATION, SECOND_JOINT_MAX_LOCATION); - - secondJointPosition = value; - secondJointPIDController.setReference( - ArmsUtil.degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getSecondJointPower()); - theStateOf2 = ArmState.PID; - secondJointCombo = 0; - - } - - // Use these for manual pid based angle increment/decrement - public void alterArmOneAngle(double degrees) { - pidForArmOne(firstJointPosition + degrees); - } - - public void alterArmTwoAngle(double degrees) { - pidForArmTwo(firstJointPosition + degrees); - } - - // PID For arm one - - // public void checkJointTwo(double value){ - // if((value + doubleDeadZone > secondJoint.getSensorPosition() && value - doubleDeadZone < secondJoint.getSensorPosition())){ - // jointOneCanContinue = true; - // }else{ - // jointOneCanContinue = false; - // } - // } - - //This is our portion with antigrav - // These next 3 antiGrav aren't used. - public void antiGravBothJoints(){ - antiGravFirstJoint(); - antiGravSecondJoint(); - } - - public void antiGravFirstJoint() { - antiGrav.updateFirstJoint(); - theStateOf1 = ArmState.ANTIGRAV; - } - - public void antiGravSecondJoint() { - antiGrav.updateSecondJoint(); - theStateOf2 = ArmState.ANTIGRAV; - } - - public void logs(){ - log("E1: " + ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - log("E2: " + ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - log("AE1: " + Math.toDegrees(altEncoder1.getPosition())); - log("AE2: " + Math.toDegrees(altEncoder2.getPosition())); - SmartDashboard.putNumber("Degree Val 1: ", ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - SmartDashboard.putNumber("Degree Val 2: ", ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - SmartDashboard.putNumber("Abs Encoder 1: ", Math.toDegrees(altEncoder1.getPosition())); - SmartDashboard.putNumber("Abs Encoder 2: ", Math.toDegrees(altEncoder2.getPosition())); - - } - - @Override - public void run() { - if(!runRateLimiter.next()) return; - IdleMode idleMode = ((CANSparkMax) secondJoint).getIdleMode(); - SmartDashboard.putString("Idle Mode", (idleMode != null) ? idleMode.toString(): "null"); - if (theStateOf1 == ArmState.PID || theStateOf2 == ArmState.PID) { - log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("First Joint Relative Encoder: " + firstJoint.getSensorPosition()); - // log("Second Joint Relative Encoder: " + secondJoint.getSensorPosition()); - // log("First Joint Difference: " + (EUTodegrees(firstJoint.getSensorPosition())-firstJointPosition)); - // log("Second Joint Difference: " + (EUTodegrees(secondJoint.getSensorPosition())-secondJointPosition)); - log("Degrees Joint 1: "+ ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - log("Degrees Joint 2: "+ ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - log("First Joint State: " + theStateOf1); - log("Second Joint State: " + theStateOf2); - log("First Joint Combo: " + firstJointCombo); - log("Second Joint Combo: " + secondJointCombo); - } - - // log("First Joint AntiGrav: "+getAntiGravFirstJoint()); - // log("Second Joint AntiGrav: "+getAntiGravSecondJoint()); - switch(theStateOf1) { - case OFF: - break; - case ANTIGRAV: - antiGravFirstJoint(); - break; - case PID: - firstJointPIDController.setReference( - ArmsUtil.degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getFirstJointPower()); - - if (Math.abs(ArmsUtil.EUTodegrees(firstJoint.getSensorPosition()) - firstJointPosition) <= doubleDeadZone){ - firstJointCombo ++; - } else { - firstJointCombo = 0; - } - - // TODO: we can actually remove this 'combo' logic since we have found that the lack of EUTodegrees made the deadzone calculation wonky - - if (firstJointCombo >= 6){ - firstJointCombo = 0; - // TODO: we do not want to do this here as arm may still be moving due to inertia - resetFirstEncoders(); - theStateOf1 = ArmState.ANTIGRAV; - } - - break; - } - - switch(theStateOf2) { - case OFF: - break; - case ANTIGRAV: - antiGravSecondJoint(); - break; - case PID: - secondJointPIDController.setReference( - ArmsUtil.degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - antiGrav.getSecondJointPower()); - - if (Math.abs(ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()) - secondJointPosition) <= doubleDeadZone){ - secondJointCombo ++; - } else { - secondJointCombo = 0; - } - - // TODO: we can actually remove this 'combo' logic since we have found that the lack of EUTodegrees made the deadzone calculation wonky - - if (secondJointCombo >= 6){ - secondJointCombo = 0; - // TODO: we do not want to do this here as arm may still be moving due to inertia - resetSecondEncoders(); - theStateOf2 = ArmState.ANTIGRAV; - } - - break; - } - - // log("First" + EUTodegrees(firstJoint.getSensorPosition()) ); - // log(" Second" + EUTodegrees(secondJoint.getSensorPosition())); - // log(theStateOf2 + ""); - // log("Difference: " + EUTodegrees(firstJoint.getSensorPosition())); - } -} - -/* ~~ Code Review ~~ - Use Voltage Control Mode when setting power (refer to CANSparkMaxMotorController.java) - Maybe use Nicholas's formula for degrees to EU - "Use break mode" - Ronald the not programmer - */ diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java b/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java deleted file mode 100644 index 93238e9b..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsAnimation.java +++ /dev/null @@ -1,67 +0,0 @@ -package com.team766.robot.mechanisms; - -import java.util.HashMap; - -public class ArmsAnimation { - - public enum AnimationState { - DISABLED, // valid next states (depending on STOW or scoring): UNSTOWED or PREP - STOWED, // valid next states: UNSTOWED - UNSTOWED, // valid next states: STOWED, PREP - PREP, // valid next states: UNSTOWED, PLAYER_STATION_CONE, MID_GOAL, HIGH_GOAL - PLAYER_STATION_CONE, // valid next states: PREP - MID_GOAL, // valid next states: PREP - HIGH_GOAL // valid next states: PREP - } - - public enum ProceedCondition { - BOTH_JOINTS_REACHED, - JOINT_ONE_REACHED_ONLY, - JOINT_TWO_REACHED_ONLY - } - - public class Waypoint { - public double jointOneAngleDegrees; - public double jointTwoAngleDegrees; - public ProceedCondition proceedCondition; - public double entryVelocity; - - public Waypoint(double jointOneAngleDegrees, double jointTwoAngleDegrees, ProceedCondition proceedCondition, double entryVelocity) { - this.jointOneAngleDegrees = jointOneAngleDegrees; - this.jointTwoAngleDegrees = jointTwoAngleDegrees; - this.proceedCondition = proceedCondition; - this.entryVelocity = entryVelocity; - } - } - - private AnimationState currentAnimationState = AnimationState.DISABLED; - private AnimationState targetAnimationState = AnimationState.DISABLED; - - public final HashMap waypoints = new HashMap(); - - public ArmsAnimation() { - // TODO: these are just estimates, calibrate on before using otherwise something may break :D - //waypoints.put(AnimationState.DISABLED, null); - //waypoints.put(AnimationState.STOWED, new Waypoint(0, -150, ProceedCondition.BOTH_JOINTS_REACHED, 500 )); - waypoints.put(AnimationState.UNSTOWED, new Waypoint(17.269, -144.387,ProceedCondition.JOINT_ONE_REACHED_ONLY, 3000)); - waypoints.put(AnimationState.PREP, new Waypoint(17.269, -90, ProceedCondition.BOTH_JOINTS_REACHED, 4000)); - waypoints.put(AnimationState.PLAYER_STATION_CONE, new Waypoint(22.73, -69.664, ProceedCondition.BOTH_JOINTS_REACHED, 3000)); - waypoints.put(AnimationState.MID_GOAL, new Waypoint(7.7765, -88.703, ProceedCondition.BOTH_JOINTS_REACHED, 3000)); - waypoints.put(AnimationState.HIGH_GOAL, new Waypoint(-17.379, -66.61, ProceedCondition. BOTH_JOINTS_REACHED, 3000)); - } - - public void update() { - // TODO - // ensure that position for desired state is kept - } - - public void changeState(AnimationState targetState) { - // valid states: DISABLED, STOWED, PLAYER_STATION_CONE, MID_GOAL, HIGH_GOAL - // switch(targetState) { - // case - // } - - // TODO : ensure that state transitions are allowed - // TODO : special case for stowing, to keep applying power - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java b/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java deleted file mode 100644 index 0c8260fe..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsAntiGrav.java +++ /dev/null @@ -1,55 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.config.ConfigFileReader; -import com.team766.hal.MotorController; -import com.team766.library.ValueProvider; - -public class ArmsAntiGrav { - - private ValueProvider ANTI_GRAV_FIRST_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravFirstJoint"); - private ValueProvider ANTI_GRAV_SECOND_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravSecondJoint"); - private static final double ANTI_GRAV_FIRSTSECOND_JOINT = 0.001; - - private MotorController firstJoint; - private MotorController secondJoint; - - public ArmsAntiGrav(MotorController j1, MotorController j2) { - firstJoint = j1; - secondJoint = j2; - } - - public void updateFirstJoint() { - firstJoint.set(getFirstJointPower()); - } - - public void updateSecondJoint() { - secondJoint.set(getSecondJointPower()); - } - - // Unused - /*private double betterGetAntiGravFirstJoint(){ - double firstRelEncoderAngle = EUTodegrees(firstJoint.getSensorPosition()); - double secondRelEncoderAngle = EUTodegrees(secondJoint.getSensorPosition()); - double massRatio = 2; //ratio between firstJoint and secondJoint - double triangleSide1 = 38; // firstJoint length - double triangleSide2 = 38; // half secondJoint length - double middleAngle = 180-(secondRelEncoderAngle-firstRelEncoderAngle); - double triangleSide3 = lawOfCosines(triangleSide1,triangleSide2,middleAngle); - double firstSecondJointAngle = firstRelEncoderAngle+lawOfSines(triangleSide3,middleAngle,triangleSide2); - double firstJointAngle = 90-Math.abs(firstRelEncoderAngle); - return (-1*Math.signum(firstRelEncoderAngle) * Math.cos((Math.PI / 180) * firstJointAngle) * ANTI_GRAV_FIRST_JOINT.valueOr(0.0)) + (-1*Math.signum(firstSecondJointAngle)*triangleSide3 * Math.sin((Math.PI / 180)*firstSecondJointAngle) * ANTI_GRAV_FIRSTSECOND_JOINT); - }*/ - - public double getFirstJointPower() { - double firstRelEncoderAngle = ArmsUtil.EUTodegrees(firstJoint.getSensorPosition()); - double firstJointAngle = 90-Math.abs(firstRelEncoderAngle); - return -1*Math.signum(firstRelEncoderAngle) * (Math.cos((Math.PI / 180) * firstJointAngle) * ANTI_GRAV_FIRST_JOINT.valueOr(0.0)); - } - - public double getSecondJointPower() { - double secondRelEncoderAngle = ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()); - double secondJointAngle = 90-Math.abs(secondRelEncoderAngle); - return -1*Math.signum(secondRelEncoderAngle) * (Math.cos((Math.PI / 180) * secondJointAngle) * ANTI_GRAV_SECOND_JOINT.valueOr(0.0)); - } - -} diff --git a/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java b/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java deleted file mode 100644 index ec731d24..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java +++ /dev/null @@ -1,58 +0,0 @@ -package com.team766.robot.mechanisms; - -/** - * Helper classes - */ -public final class ArmsUtil { - - /** - * Static class only - */ - private ArmsUtil() { - } - - public static double degreesToEU(double angle) { - // return angle * (44.0 / 90) * (25.0 / 16.0); - // Nicholas' accuracy fix - return angle * (75.0 / 94.0); - } - - public static double EUTodegrees(double EU) { - // return EU * (90 / 44.0) * (16.0 / 25.0); - // Nicholas' accuracy fix - return EU * (94.0 / 75.0); - } - - public static double AbsToEU(double abs) { - return degreesToEU(360 * abs); - } - - public static double EUToAbs(double EU) { - return EUTodegrees(EU) / 360.0; - } - - /** - * Cosine law - * @param side1 - * @param side2 - * @param angle in degrees - * @return - */ - public static double lawOfCosines(double side1, double side2, double angle) { - double side3Squared = (Math.pow(side1,2.0)+Math.pow(side2,2.0)-2*side1*side2*Math.cos(Math.toRadians(angle))); - return Math.sqrt(side3Squared); - } - - public static double lawOfSines(double side1, double angle1, double side2) { - return Math.asin(side2*Math.sin(angle1)/side1); - } - - public static double clampValueToRange(double value, double min, double max) { - if(value > max){ - value = max; - } else if( value < min){ - value = min; - } - return value; - } -} diff --git a/src/main/java/com/team766/robot/mechanisms/CANdleMech.java b/src/main/java/com/team766/robot/mechanisms/CANdleMech.java deleted file mode 100644 index 1b8f6788..00000000 --- a/src/main/java/com/team766/robot/mechanisms/CANdleMech.java +++ /dev/null @@ -1,50 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.RobotProvider; -import com.team766.logging.Severity; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.util.Color8Bit; - -import com.team766.hal.MotorController; - -import java.io.*; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.util.*; - -import javax.imageio.ImageIO; - -import com.ctre.phoenix.led.*; -import com.ctre.phoenix.led.CANdle.LEDStripType; -import com.ctre.phoenix.led.CANdle.VBatOutputMode; -import com.ctre.phoenix.led.ColorFlowAnimation.Direction; -import com.ctre.phoenix.led.LarsonAnimation.BounceMode; -import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; -import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent; -import java.io.File; -import java.io.IOException; -import java.nio.file.Files; -import java.awt.image.BufferedImage; -import java.awt.image.Raster; - -public class CANdleMech extends Mechanism { - - private final CANdle m_candle = new CANdle(5); - - public CANdleMech() { - - } - - public void setColor(short r, short g, short b, int index, int count) { - checkContextOwnership(); - m_candle.setLEDs(r / 5, g / 5, b / 5, 0, index, count); - } - - public void setColor(int r, int g, int b) { - checkContextOwnership(); - m_candle.setLEDs(r / 5, g / 5, b / 5); - } - -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 8bad93c9..33b1f0e4 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -11,9 +11,11 @@ import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.simulator.ProgramInterface.RobotMode; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; +import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; @@ -23,6 +25,7 @@ import com.team766.odometry.PointDir; import com.team766.hal.MotorControllerCommandFailedException; import com.team766.robot.constants.*; +import com.team766.controllers.*; public class Drive extends Mechanism { @@ -52,9 +55,23 @@ public class Drive extends Mechanism { private Point[] wheelPositions; private Odometry swerveOdometry; + + private PIDController pidX; + private PIDController pidY; + private PIDController pidPythag; + + private boolean yDone; + public Drive() { loggerCategory = Category.DRIVE; + + yDone = false; + + pidX = new PIDController(0.1, 0, 0, 0, -0.2, 0.2, 0.5); + pidY = new PIDController(0.5, 0, 0, 0, -0.2, 0.2, 0.5); + pidPythag = new PIDController(0.2, 0, 0, 0, 0.2, -0.2, 0.1); + // Initializations of motors // Initialize the drive motors m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); @@ -165,7 +182,7 @@ public double getAngle(double LR, double FB) { */ public boolean withinHalfACircle(double angle1, double angle2) { angle1 = mod(angle1, 360); - angle2 = mod(angle2, 360); + angle2 = mod(angle2, 360); if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) { angle2 += 360; } @@ -363,6 +380,78 @@ public void stopSteerMotors() { m_SteerBackLeft.stopMotor(); } + public int PhotonDrive(Transform3d bestTrackedTarget, PhotonTrackedTarget notBestTarget){ + double curX = bestTrackedTarget.getX(); + double curY = bestTrackedTarget.getY(); + log("cur: " + curX + " e: " + curY); + double curAng = notBestTarget.getSkew(); + + double pythagorean = curX * curX + curY * curY; + double sqrtPythag = Math.sqrt(pythagorean); + double angleB = Math.asin(1/sqrtPythag*curX); + + pidX.setSetpoint(0); + pidY.setSetpoint(0); + pidPythag.setSetpoint(0); + + pidX.calculate(curX); + pidY.calculate(curY); + pidPythag.calculate(sqrtPythag); + + double outX = -1 * pidX.getOutput(); + double outY = pidY.getOutput(); + double outPythag = -1 * pidPythag.getOutput(); + + log("X: " + outX); + log("Y: " + outY); + + if(curY <=0.2 && curY >= -0.2){ + yDone = true; + }else{ + yDone = false; + } + + // setAllAngles(angleB); + + // m_DriveBackLeft.set(outPythag); + // m_DriveBackRight.set(outPythag); + // m_DriveFrontLeft.set(outPythag); + // m_DriveFrontRight.set(outPythag); + + if(yDone){ + setAllAngles(180); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outX); + m_DriveBackRight.set(outX); + m_DriveFrontLeft.set(outX); + m_DriveFrontRight.set(outX); + }else{ + setAllAngles(90); //TODO: CHECK IF IT IS CORRECT ANGLE AND NOT SWITCHED + m_DriveBackLeft.set(outY); + m_DriveBackRight.set(outY); + m_DriveFrontLeft.set(outY); + m_DriveFrontRight.set(outY); + } + // log("currAng: " + curAng); + // if(Math.abs(curAng) <= 2){ + // //do nothing + // }else if(outX == 0 && outY == 0){ + // if(curAng > 0){ + // setAllAngles(curAng - 0.15); + // }else{ + // setAllAngles(curAng + 0.15); + // } + // } + + if(curY <=0.2 && curY >= -0.2 && curX <= 0.2 && curX >= -0.2){ + log("DONE"); + return 1; + }else{ + log("Q: " + curY + " : " + curX); + return 0; + } + + } + /** * This method is the main method for driving the robot, using the joystick values. * @@ -450,6 +539,7 @@ public void swerveDrive(PointDir joystick) { swerveDrive(-1 * joystick.getY(), -1 * joystick.getX(), joystick.getHeading()); } + /** * This method is used to simply turn the robot without moving it * @@ -578,6 +668,17 @@ public void setBackLeftAngle(double angle) { m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); } +/** + * This is a method to set all of the motors to a certian angle + * + * @param angle The angle to set all of the wheels to + */ + public void setAllAngles(double angle){ + setBackLeftAngle(angle); + setBackRightAngle(angle); + setFrontLeftAngle(angle); + setFrontRightAngle(angle); + } /** * Method to configure PID values. The values were pre-tuned and are not expected to change. */ diff --git a/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java b/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java deleted file mode 100644 index 75f24df9..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ExampleMechanism.java +++ /dev/null @@ -1,22 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; - -public class ExampleMechanism extends Mechanism { - private MotorController leftMotor; - private MotorController rightMotor; - - public ExampleMechanism() { - leftMotor = RobotProvider.instance.getMotor("exampleMechanism.leftMotor"); - rightMotor = RobotProvider.instance.getMotor("exampleMechanism.rightMotor"); - } - - public void setMotorPower(double leftPower, double rightPower){ - checkContextOwnership(); - - leftMotor.set(leftPower); - rightMotor.set(rightPower); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Grabber.java b/src/main/java/com/team766/robot/mechanisms/Grabber.java deleted file mode 100644 index 4befedb7..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Grabber.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; - -public class Grabber extends Mechanism { - private MotorController grabby; - - public Grabber(){ - grabby = RobotProvider.instance.getMotor("arms.grabber"); - grabby.setCurrentLimit(10.0); - } - - public void grabberPickUp(){ - checkContextOwnership(); - grabby.set(1.0); - - } - - public void grabberLetGo(){ - checkContextOwnership(); - grabby.set(-0.5); - } - - public void grabberStop(){ - checkContextOwnership(); - grabby.set(0.0); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java deleted file mode 100644 index 5fec31d6..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Intake.java +++ /dev/null @@ -1,79 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; -import com.team766.hal.SolenoidController; -import com.team766.hal.wpilib.CANSparkMaxMotorController; -import com.team766.hal.wpilib.Solenoid; -import edu.wpi.first.wpilibj.PneumaticHub; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Intake extends Mechanism { - - private MotorController bottomWheels; - private MotorController topBelt; - private SolenoidController leftPiston; - private SolenoidController rightPiston; - private PneumaticHub ph; - - public Intake() { - topBelt = RobotProvider.instance.getMotor("intake.topWheels"); - bottomWheels = RobotProvider.instance.getMotor("intake.bottomWheels"); - - leftPiston = RobotProvider.instance.getSolenoid("intake.leftPiston"); - rightPiston = RobotProvider.instance.getSolenoid("intake.rightPiston"); - ph = new PneumaticHub(); - - } - - public void startIntake() { - checkContextOwnership(); - - pistonsOut(); - topBelt.set(1.0); - bottomWheels.set(1.0); - - } - - public void intakePistonless() { - topBelt.set(1.0); - bottomWheels.set(1.0); - } - - public void stopIntake() { - checkContextOwnership(); - - topBelt.set(0.0); - bottomWheels.set(0.0); - pistonsIn(); - } - - public void pistonsOut() { - checkContextOwnership(); - log("pistons out"); - - leftPiston.set(true); - rightPiston.set(true); - } - - public void pistonsIn() { - checkContextOwnership(); - - leftPiston.set(false); - rightPiston.set(false); - } - - public void reverseIntake() { - checkContextOwnership(); - - topBelt.set(-1.0); - bottomWheels.set(-1.0); - - pistonsIn(); - } - - public void run(){ - SmartDashboard.putNumber("Storage PSI",ph.getPressure(0)); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTag.java b/src/main/java/com/team766/robot/mechanisms/januaryTag.java new file mode 100644 index 00000000..1c8d5c73 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/januaryTag.java @@ -0,0 +1,288 @@ +package com.team766.robot.mechanisms; + +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.config.ConfigFileReader; +import com.team766.controllers.PIDController; +import com.team766.framework.Context; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.hal.RobotProvider; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +//import com.team766.logging.Category; +import de.erichseifert.gral.util.GeometryUtils; +import de.erichseifert.gral.util.MathUtils; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.util.*; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import javax.tools.ForwardingFileObject; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class januaryTag extends Mechanism{ + private PhotonCamera camera1; + private double deadzoneX; + private double deadzoneY; + private double x_target; + private double y_target; + private MotorController leftMotor; + private MotorController rightMotor; + private double x_targetConstant; + private double turnConstant; + private double forwardConstant; + private double offsetX; + private double offsetY; + private Transform3d offset; + private Drive drive; + + + private PIDController motionX; + private PIDController motionY; + public januaryTag(){ + + motionX = new PIDController(0.0,0.0,0.0,-0.2,0.2,0.5); + motionY = new PIDController(0,0,0,-0.2,0.2,1); + drive = new Drive(); + loggerCategory = Category.MECHANISMS; + camera1 = new PhotonCamera("januaryTag"); + leftMotor = RobotProvider.instance.getMotor("leftMotor"); + rightMotor = RobotProvider.instance.getMotor("rightMotor"); + + //Sample values, we don't know these work at all yet + x_targetConstant = 0.5; + turnConstant = 0.3; + forwardConstant = 0.2; + offsetX = 0.75; + offsetY = 0; + offset = new Transform3d(new Translation3d(-offsetX, -offsetY, 0), new Rotation3d()); + checkContextOwnership(); + + motionX.setP(0.01); + motionY.setP(0.01); + + } + //Manually move the robot + public void manual(double left, double right){ + leftMotor.set(left); + rightMotor.set(right); + } + + public Transform3d getBestTag(){ + return getBestCameraToTarget(getBestTrackedTarget()); + } + + + public void swerveCalculate(){ + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + + double xCurPos = targetTransform.getX(); + double yCurPos = targetTransform.getY(); + + motionX.setSetpoint(0); + motionY.setSetpoint(0); + + motionX.calculate(xCurPos); + motionY.calculate(yCurPos); + + double xSpeed = motionX.getOutput(); + double ySpeed = motionY.getOutput(); + + drive.drive2D(xSpeed, ySpeed); + + } + + + /* + * This method returns the best target that the camera is currently tracking + * This is useful for the transform3d data + * @return PhotonTrackedTarget - the best target that the camera is currently tracking + */ + public PhotonTrackedTarget getBestTrackedTarget(){ + var result = camera1.getLatestResult(); //getting the result from the camera + boolean hasTargets = result.hasTargets(); // checking to see if there are any targets in the camera's view. IF THERE ISN'T AND YOU USE result.getTargets() YOU WILL GET AN ERROR + + if(hasTargets){ + List targets = result.getTargets(); // getting targets + + PhotonTrackedTarget bestTrackedTarget = result.getBestTarget(); // getting the best target that is currently being picked up by the camera so that it can know where it is + return bestTrackedTarget; + }else{ + log("No targets? see what i did there"); + throw new januaryTagException("There were no targets that could be picked up by the camera, so I'm gonna have to throw this error here."); + } + } + + public void setXtargetConstant(double constant){ + x_targetConstant = constant; + } + + /* + * For all next three methods, the constants are the constants that are used in the arcade drive method + * You can set them individually or together + */ + + public void setTurnConstant(double constant){ + turnConstant = constant; + } + + public void setForwardConstant(double constant){ + forwardConstant = constant; + } + + public void setArcadeDriveVisionConstants(double turn, double forward){ + turnConstant = turn; + forwardConstant = forward; + } + + /* + * This method returns the Transform3d data from the best target that the camera is currently tracking + * @param target - the target that you want the camera to get the data from (use getBestTrackedTarget() to get the best target that the camera is currently tracking) + * @return Transform3d - the Transform3d from the best target that the camera is currently tracking + */ + + public Transform3d getBestCameraToTarget(PhotonTrackedTarget target){ + return target.getBestCameraToTarget(); + } + + /* + * This method returns the data from the best target that the camera is currently tracking, not the Transform3d data + * @return ArrayList - the data from the best target that the camera is currently tracking + * [0] is the yaw + * [1] is the pitch + * [2] is the area that the apriltag fills the camera view with + * [3] is the fiducial id of the april tag + */ + public ArrayList getPhotonTrackedTargetData(){ + ArrayList arr = new ArrayList(); + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + arr.add(theTarget.getYaw()); + arr.add(theTarget.getPitch()); + arr.add(theTarget.getArea()); + arr.add((double) theTarget.getFiducialId()); + return arr; + } + /* + * This method returns the ID of the target that the camera is currently tracking + * It could be used to tell which target it is aimed at so we know where the robot is + * @return the ID of the target that the camera is currently tracking + */ + public int getJanuaryTagId(PhotonTrackedTarget theTarget){ + return theTarget.getFiducialId(); + } + + /* + * This is a method to return data given from a Transform3d + * @return an ArrayList with values about the Transform3d, assuming the camera is (0,0) + * [0] is the x value of the target relative to the camera + * [1] is the y value of the target relative to the camera + * [2] is the z value of the target relative to the camera + */ + public ArrayList getTransform3dData(){ + ArrayList arr = new ArrayList(); + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + arr.add(targetTransform.getX()); + arr.add(targetTransform.getY()); + arr.add(targetTransform.getZ()); + return arr; + } + + /* + * These next three methods are for setting deadzones for the go() method. + * For the first two methods @param dz - the deadzone that you want to set + * For the last method @param x - the x deadzone that you want to set, @param y - the y deadzone that you want to set + */ + + public void setDeadzoneX(double dz){ + deadzoneX = dz; + } + + public void setDeadzoneY(double dz){ + deadzoneY = dz; + } + + public void setDeadzones(double x, double y){ + deadzoneX = x; + deadzoneY = y; + } + /* + * This method is the method that you should call to make the robot go to the target. Currently, it will go to any apriltag that it sees, but we can change that later. + * It uses the getBestTrackedTarget() method to get the best target that the camera is currently tracking, and then uses it in the Transform3d object. + * It then uses the Transform3d object to get the x and y values of the target relative to the camera, and then uses those values to calculate the x and y values of the target relative to the robot. + * If the x and y values of the target relative to the robot are within the deadzone, set in OI, then the robot will stop moving. + * If the x and y values of the target relative to the robot are not within the deadzone, then the robot will move towards the target using arcade drive formulas. + */ + public void go(){ + + while(2>1){ + + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + Transform3d scoring = targetTransform.plus(offset); + + if(scoring.getX() + deadzoneX > 0 && scoring.getX() - deadzoneX < 0 && scoring.getY() + deadzoneY > 0 && scoring.getY() - deadzoneY < 0){ + log("Outtaking"); + log("FINAL X: " + scoring.getX()); + log("FINAL Y: " + scoring.getY()); + break; + }else{ + + + double x_scoring = scoring.getX(); + double y_scoring = scoring.getY(); + + double yaw_scoring = scoring.getRotation().getZ(); + double x_target = x_scoring + Math.cos(yaw_scoring) * (x_targetConstant * y_scoring); + double y_target = y_scoring + Math.sin(yaw_scoring) * (x_targetConstant * y_scoring); + + + double forward = Math.sqrt((y_target * y_target) + (x_target * x_target)); + double turn = -Math.tan(y_target/x_target); + + + double leftMotorPower = turnConstant * turn + forwardConstant * MathUtil.clamp(forward, -1, 1); + double rightMotorPower = turnConstant * -turn + forwardConstant * MathUtil.clamp(forward, -1, 1); + + leftMotor.set((-1) * leftMotorPower); + rightMotor.set((-1) * rightMotorPower); + + log("X: " + scoring.getX()); + log("Y: " + scoring.getY()); + } + } + } + + /* + * These are debug logs + */ + public void debugLogs(){ + Transform3d targetTransform = getBestCameraToTarget(getBestTrackedTarget()); + PhotonTrackedTarget theTarget = getBestTrackedTarget(); + + log("Yaw PTT " + theTarget.getYaw()); + log("Pitch PTT " + theTarget.getPitch()); + log("Yaw: " + targetTransform.getX()); + log("Pitch: " + targetTransform.getY()); + } + +} diff --git a/src/main/java/com/team766/robot/mechanisms/januaryTagException.java b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java new file mode 100644 index 00000000..f423585d --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/januaryTagException.java @@ -0,0 +1,13 @@ +package com.team766.robot.mechanisms; + +public class januaryTagException extends RuntimeException { + public januaryTagException (String errorMessage) { + super(errorMessage); + } +} + +public class januaryTagException extends RuntimeException { + public januaryTagException (String errorMessage) { + super(errorMessage); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/AlignCharger.java b/src/main/java/com/team766/robot/procedures/AlignCharger.java deleted file mode 100644 index 90cc2a3b..00000000 --- a/src/main/java/com/team766/robot/procedures/AlignCharger.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.framework.Context; -import com.team766.robot.Robot; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// Procedure to align robot to a position where it can easily balance on the charge station -// See AlignCharger.md for more details -public class AlignCharger extends Procedure { - - private Alliance alliance; - - - - public void run(Context context) { - context.takeOwnership(Robot.drive); - // Uses the ChargeStationPathFinder helper class to find correct points - ChargeStationPathFinder finder = new ChargeStationPathFinder(alliance); - PointDir[] points = finder.calculatePoints(Robot.drive.getCurrentPosition()); - - // Calls FollowPoints on the calculated points - context.startAsync(new FollowPoints(points)); - } - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public AlignCharger(Alliance alliance) { - this.alliance = alliance; - } - - -} diff --git a/src/main/java/com/team766/robot/procedures/AutoScoring.java b/src/main/java/com/team766/robot/procedures/AutoScoring.java deleted file mode 100644 index a68a5f1f..00000000 --- a/src/main/java/com/team766/robot/procedures/AutoScoring.java +++ /dev/null @@ -1,86 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.robot.procedures.FollowPoints; -import com.team766.robot.Robot; -import com.team766.robot.RobotTargets; -/** - * This procedure will eventually be able to score game pieces automatically. - * Currently, it moves the robot to the nearest scoring node. - * Once methods for other scoring mechanisms are done, they should be added here so this procedure wil be able to score completely autonomously. - */ -public class AutoScoring extends Procedure{ - Point currentPos; - double minDistance; - Point targetPoint; - Nodes targetNode; - Piece targetPiece; - Point[] pointList; - - public enum Nodes { - HYBRID, - MEDIUM, - HIGH - } - - public enum Piece { - HYBRID, - CUBE, - CONE - } - - public AutoScoring(Nodes node) { - this(node, Piece.HYBRID); - - if (node == Nodes.HYBRID) { - targetPiece = Piece.HYBRID; - } else { - targetPiece = Piece.CONE; - for (int i = 0; i < RobotTargets.CUBE_ROWS.length; i++) { - if (targetPoint.getY() == RobotTargets.CUBE_ROWS[i]) { - targetPiece = Piece.CUBE; - } - } - } - } - - public AutoScoring(Nodes node, Piece piece) { - - - switch (piece) { - case CUBE: pointList = RobotTargets.CUBE_NODES; - case CONE: pointList = RobotTargets.CONE_NODES; - default: pointList = RobotTargets.NODES; - } - - targetPiece = piece; - targetPoint = pointList[0]; - targetNode = node; - - } - - public void run(Context context) { - log("Starting AutoScoring " + targetPoint.toString()); - - currentPos = Robot.drive.getCurrentPosition().clone(); - minDistance = currentPos.distance(pointList[0]); - log("First Distance: " + minDistance); - - - for (int i = 1; i < pointList.length; i++) { - if (currentPos.distance(pointList[i]) < minDistance) { - minDistance = currentPos.distance(pointList[i]); - targetPoint = pointList[i]; - } - } - log("Final Distance: " + minDistance); - - - context.waitFor(context.startAsync(new FollowPoints(Robot.drive.getCurrentPosition().clone(), new PointDir[]{new PointDir(targetPoint, 0)}))); - //Do the actual scoring (arm movements, etc.) - log("Finishing AutoScoring"); - } -} diff --git a/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java b/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java deleted file mode 100644 index 62c7b13d..00000000 --- a/src/main/java/com/team766/robot/procedures/ChargeStationPathFinder.java +++ /dev/null @@ -1,86 +0,0 @@ -package com.team766.robot.procedures; - -import java.util.ArrayList; -import java.util.List; -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.odometry.PointDir; -import com.team766.robot.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// Helper class for AlignCharger, see AlignCharger.md for more details -public class ChargeStationPathFinder { - - // Since this class does not extend Procedure, we need to instantiate logger - private static final Logger logger = Logger.get(Category.PROCEDURES); - private final Alliance alliance; - - /** - * Constructor which takes alliance - * @param alliance Alliance for choosing which charge station to align to - */ - public ChargeStationPathFinder(Alliance alliance) { - this.alliance = alliance; - } - - /** - * Calculates points that the robot needs to follow based on alliance and current position - * @param curPos the current robot position, represented by a pointDir - * @return PointDir[]: list of the calculated points that can be entered into FollowPoints - */ - public PointDir[] calculatePoints(PointDir curPos) { - - // Sets curX and curY variables based on curPos param - double curX = curPos.getX(); - double curY = curPos.getY(); - - // Creates ArrayList for points - List points = new ArrayList(); - switch (alliance) { - case Red: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.RED_BALANCE_TARGET_X, ChargeConstants.RED_LEFT_PT, ChargeConstants.RED_RIGHT_PT, ChargeConstants.MIDDLE); - break; - - case Blue: - // Calls addPoints method for red alliance coordinates - addPoints(points, curX, curY, ChargeConstants.BLUE_BALANCE_TARGET_X, ChargeConstants.BLUE_LEFT_PT, ChargeConstants.BLUE_RIGHT_PT, ChargeConstants.MIDDLE); - break; - - case Invalid: // drop down - default: - logger.logRaw(null, "Invalid Alliance"); - } - - // Converts pointDir arrayList to array and returns it - return points.toArray(new PointDir[points.size()]); - } - - /** - * Adds a set of points to an arrayList based on passed charge station coordinates - * @param points ArrayList to add points to - * @param curX Current robot X value - * @param curY Current robot Y value - * @param target X value of center of target charge station - * @param left X value of left side of target charge station - * @param right X value of right side of target charge station - * @param height Y value of center of target charge station - */ - private void addPoints(List points, double curX, double curY, double target, double left, double right, double height) { - // If on the right side of the target, check if robot is blocked by the right side of the charge station, and add points accordingly - if (curX > target) { - if (curX < right) - points.add(new PointDir(right, curY)); - - points.add(new PointDir(right, height)); - - // Otherwise, it will be on the left side of the target, so check if robot is blocked by the left side of the charge station, and add points accordingly - } else { - if (curX > left) - points.add(new PointDir(left, curY)); - - points.add(new PointDir(left, height)); - } - } - -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/DoNothing.java b/src/main/java/com/team766/robot/procedures/DoNothing.java index 566ad756..1e2e3177 100644 --- a/src/main/java/com/team766/robot/procedures/DoNothing.java +++ b/src/main/java/com/team766/robot/procedures/DoNothing.java @@ -1,11 +1,9 @@ package com.team766.robot.procedures; -import com.team766.framework.Context; -import com.team766.framework.Procedure; +public class DoNothing { -public class DoNothing extends Procedure { - - public void run(Context context) { + public DoNothing(){ + } -} \ No newline at end of file +} diff --git a/src/main/java/com/team766/robot/procedures/FollowPoints.java b/src/main/java/com/team766/robot/procedures/FollowPoints.java deleted file mode 100644 index be571a11..00000000 --- a/src/main/java/com/team766/robot/procedures/FollowPoints.java +++ /dev/null @@ -1,482 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Procedure; -import com.team766.framework.Context; -import com.team766.framework.LaunchedContext; -import com.team766.robot.Robot; -import com.team766.robot.constants.*; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.hal.RobotProvider; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.hal.PositionReader; -import java.io.File; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import com.team766.config.ConfigFileReader; -import com.team766.logging.Category; -import com.team766.logging.Severity; -import com.team766.controllers.PIDController; -import com.team766.robot.procedures.*; -import edu.wpi.first.wpilibj.Filesystem; -import org.json.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -/** - * {@link Procedure} to follow a set of waypoints. Waypoint files can be passed in via - * the {@link #FollowPoints(String)} constructor. - */ -public class FollowPoints extends Procedure { - - //Steps combine possible data types into one object for flexibility and ease-of-use purposes - public static class Step { - - public PointDir wayPoint; - public boolean criticalPoint; - public Procedure procedure; - public boolean stopRobot; - // ... - - public Step(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) { - this.wayPoint = wayPoint; - this.procedure = procedure; - this.criticalPoint = criticalPoint; - this.stopRobot = stopRobot; - } - } - - private List steps = new ArrayList(); - - private static PointDir currentPos = new PointDir(0.0, 0.0, 0.0); - private static PointDir lastPos = new PointDir(0.0, 0.0, 0.0); - private static PointDir lastPos2 = new PointDir(0.0, 0.0, 0.0); - - private PointDir[] pointList; - private Procedure[] proceduresAtPoints; - private boolean[] criticalPointList; - private boolean[] stopRobotList; - private Point startingPoint = new Point(0, 0); - - private int targetNum = 0; - private RateLimiter followLimiter = new RateLimiter(FollowPointsInputConstants.RATE_LIMITER_TIME); - - //Radius defines the radius of the circle around the robot - private static double radius = FollowPointsInputConstants.RADIUS; - private static double speed = FollowPointsInputConstants.SPEED; - private static PointDir driveSettings = new PointDir(0, 0, 0); - - private static HashMap mapOfProcedures = new HashMap(); - - /*public FollowPoints() { - parsePointL$ist(); - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - loggerCategory = Category.AUTONOMOUS; - }*/ - - /** - * Constructor to create a new robot-oriented FollowPoints instance. - * @param file filename for the waypoints file to load and use. - * @throws IOException Thrown if file is not found. - */ - public FollowPoints(String file) { - mapOfProcedures.put("DoNothing()", new DoNothing()); - mapOfProcedures.put(null, new DoNothing()); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.HIGH)", new AutoScoring(AutoScoring.Nodes.HIGH)); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.MEDIUM)", new AutoScoring(AutoScoring.Nodes.MEDIUM)); - mapOfProcedures.put("AutoScoring(AutoScoring.Nodes.HYBRID)", new AutoScoring(AutoScoring.Nodes.HYBRID)); - mapOfProcedures.put("setCross()", new setCross()); - - String str; - Path path = Filesystem.getDeployDirectory().toPath().resolve(file); - try { - str = Files.readString(path); - } catch (IOException e) { - e.printStackTrace(); - log(Severity.ERROR, "Could not load " + file); - return; - } - - JSONArray points = new JSONObject(str).getJSONArray("points"); - JSONArray start = new JSONObject(str).getJSONArray("start"); - - startingPoint.set(start.getDouble(0), start.getDouble(1)); - - for (int i = 0; i < points.length(); i++) { - JSONObject procedure = points.getJSONObject(i).getJSONObject("procedure"); - Procedure pointProcedure = null; - boolean stopAtProcedure = false; - if (procedure != null) { - pointProcedure = mapOfProcedures.get(procedure.getString("name")); - stopAtProcedure = procedure.getBoolean("stop"); - } - addStep(new PointDir(points.getJSONObject(i).getJSONArray("coordinates").getDouble(0), points.getJSONObject(i).getJSONArray("coordinates").getDouble(1), points.getJSONObject(i).getJSONArray("coordinates").getDouble(2)), points.getJSONObject(i).getBoolean("critical"), pointProcedure, stopAtProcedure); - } - addWaypoints(); - } - - /** - * Constructor to create a new field-oriented FollowPoints instance. - * @param startingPoint The original position of the robot - * @param file filename for the waypoints file to load and use. - * @throws IOException Thrown if file is not found. - */ - public FollowPoints(Point startingPoint, String file) { - this.startingPoint = startingPoint; - String str; - Path path = Filesystem.getDeployDirectory().toPath().resolve(file); - try { - str = Files.readString(path); - } catch (IOException e) { - e.printStackTrace(); - log(Severity.ERROR, "Could not load " + file); - return; - } - - JSONArray points = new JSONObject(str).getJSONArray("points"); - for (int i = 0; i < points.length(); i++) { - JSONObject procedure = points.getJSONObject(i).getJSONObject("procedure"); - Procedure pointProcedure = null; - boolean stopAtProcedure = false; - if (procedure != null) { - pointProcedure = mapOfProcedures.get(procedure.getString("name")); - stopAtProcedure = procedure.getBoolean("stop"); - } - addStep(new PointDir(points.getJSONObject(i).getJSONArray("coordinates").getDouble(0), points.getJSONObject(i).getJSONArray("coordinates").getDouble(1), points.getJSONObject(i).getJSONArray("coordinates").getDouble(2)), points.getJSONObject(i).getBoolean("critical"), pointProcedure, stopAtProcedure); - } - addWaypoints(); - } - - /** - * Method which creates a new Step object from its constituents. - * @param wayPoint PointDir consisting of the x- and y- coordinates of the point, as well as the target header for the robot at that point. - * @param criticalPoint Boolean determining whether the robot should travel to the point (true) or if it should use the FollowPoints circle method (false). - * @param procedure Procedure for the robot to run when reaching the point. If null is inputted, it is interpreted as "doNothing()". - * @param stopRobot Boolean determining whether the robot should wait for the procedure to finish before continuing on its path. - */ - private void addStep(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) { - steps.add(new Step(wayPoint, criticalPoint, procedure, stopRobot)); - } - - /** - * Default FollowPoints Constructor. Steps must be added in-code. This is robot-oriented. - */ - public FollowPoints() { - addStep(new PointDir(0,0, 0), false, new DoNothing(), false); - addStep(new PointDir(2,0, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(2,2, 0), false, new DoNothing(), false); - addStep(new PointDir(0,2, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(0,0, 0), true, new DoNothing(), false); - addWaypoints(); - } - - /** - * Default FollowPoints Constructor but with starting point. Steps must be added in-code. This is field-oriented. - */ - public FollowPoints(Point startingPoint) { - this.startingPoint = startingPoint; - addStep(new PointDir(0,0, 0), false, new DoNothing(), false); - addStep(new PointDir(2,0, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(2,2, 0), false, new DoNothing(), false); - addStep(new PointDir(0,2, 90), false, null /* don't execute procedure */, false); - addStep(new PointDir(0,0, 0), true, new DoNothing(), false); - addWaypoints(); - } - - /** - * When using steps (in the default constructor), this sets up the arrays to be used by the FollowPoints method. - */ - private void addWaypoints() { - pointList = new PointDir[steps.size()]; - proceduresAtPoints = new Procedure[steps.size()]; - stopRobotList = new boolean[pointList.length]; - criticalPointList = new boolean[pointList.length]; - for (int i = 0; i < steps.size(); i++) { - if (steps.get(i).wayPoint == null) continue; - else { - pointList[i] = steps.get(i).wayPoint; - } - if (steps.get(i).procedure != null) { - proceduresAtPoints[i] = steps.get(i).procedure; - // run procedure - // TODO: make sure we're handling contexts correctly - } else { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList[i] = steps.get(i).criticalPoint; - - stopRobotList[i] = steps.get(i).stopRobot; - } - } - - - /** - * Constructor which takes an array of points, and is robot-oriented. - * @param points Array of PointDir objects for the robot to follow, consisting of x- and y- coordinates, as well as a target header for the robot. - */ - public FollowPoints(PointDir[] points) { - pointList = points; - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList = new boolean[pointList.length]; - stopRobotList = new boolean[pointList.length]; - loggerCategory = Category.AUTONOMOUS; - } - - /** - * Constructor which takes an array of points, and is field-oriented. - * @param points Array of PointDir objects for the robot to follow, consisting of x- and y- coordinates, as well as a target header for the robot. - */ - public FollowPoints(Point startingPoint, PointDir[] points) { - this.startingPoint = startingPoint; - pointList = points; - proceduresAtPoints = new Procedure[pointList.length]; - for (int i = 0; i < proceduresAtPoints.length; i++) { - proceduresAtPoints[i] = new DoNothing(); - } - criticalPointList = new boolean[pointList.length]; - stopRobotList = new boolean[pointList.length]; - loggerCategory = Category.AUTONOMOUS; - } - - //Takes pairs of points from pointDoubles (set in the config file) and converts them to Points, which are placed in pointList. - /*private void parsePointList() { - Double[] pointDoubles = ConfigFileReader.getInstance().getDoubles("trajectory.points").get(); - pointList = new PointDir[pointDoubles.length / 2]; - double pointX = 0; - double pointY = 0; - for (int i = 0; i < pointDoubles.length / 2 * 2; i++) { - if (i % 2 == 0) - pointX = pointDoubles[i]; - else { - pointY = pointDoubles[i]; - pointList[i / 2] = new PointDir(pointX, pointY); - } - } - }*/ - - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - log("Starting FollowPoints"); - - if (startingPoint == null) { - for (int i = 0; i < pointList.length; i++) { - pointList[i].add(Robot.drive.getCurrentPosition()); - } - } else { - Robot.drive.setCurrentPosition(startingPoint); - } - targetNum = 0; - - for (int i = 0; i < pointList.length; i++) { - log(pointList[i].toString()); - log("Stop: " + stopRobotList[i]); - log("Procedure: " + proceduresAtPoints[i]); - } - if (pointList.length > 0) { - Point targetPoint = new Point(0.0, 0.0); - currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading()); - while (targetNum < pointList.length - 1 || !passedPoint(pointList[pointList.length - 1])) { - if (followLimiter.next()) { - lastPos2 = lastPos.clone(); - lastPos = currentPos.clone(); - currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading()); - //If the next point is a critical point, the robot will wait until it has passed that point for it to move to the next point - //Otherwise, it uses the checkIntersection() method to follow the circle - if (criticalPointList[targetNum]? (targetNum < pointList.length - 1 && passedPoint(pointList[targetNum])) : checkIntersection(pointList)) { - if (targetNum != stopRobotList.length - 1 && proceduresAtPoints.length > targetNum) { - if (stopRobotList[targetNum]) { - driveSettings.set(0, 0); - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - Robot.drive.setCross(); - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - context.waitFor(context.startAsync(proceduresAtPoints[targetNum])); - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - } else { - context.startAsync(proceduresAtPoints[targetNum]); - } - } - targetNum++; - log("Going to Next Point! " + pointList[targetNum]); - } - targetPoint = selectTargetPoint(targetNum, pointList); - //double diff = currentPos.getAngleDifference(targetPoint); - //Robot.drive.setDrivePower(straightVelocity + Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity), straightVelocity - Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity)); - - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - driveSettings.set(currentPos.scaleVector(targetPoint, speed), rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())); - driveSettings.set((DriverStation.getAlliance() == Alliance.Blue ? -1 : 1) * driveSettings.getX(), (DriverStation.getAlliance() == Alliance.Blue ? 1 : -1) * driveSettings.getY()); - Robot.drive.swerveDrive(driveSettings); - //log("Current Position: " + currentPos.toString()); - //log("Target Point: " + targetPoint.toString()); - //log("Unit Vector: " + new PointDir(currentPos.scaleVector(targetPoint, speed), rotationSpeed(Robot.gyro.getGyroYaw(), pointList[targetNum].getHeading())).toString()); - - context.yield(); - } else { - updateRotation(); - } - } - if (proceduresAtPoints.length > targetNum) { - if (stopRobotList[stopRobotList.length - 1]) { - driveSettings.set(0, 0); - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[stopRobotList.length - 1].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - Robot.drive.setCross(); - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - context.waitFor(context.startAsync(proceduresAtPoints[stopRobotList.length - 1])); - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.gyro); - } else { - context.startAsync(proceduresAtPoints[stopRobotList.length - 1]); - } - } - Robot.drive.drive2D(0, 0); - driveSettings.set(0, 0); - targetNum = pointList.length - 1; - while (Math.abs(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())) > 0.03) { - updateRotation(); - context.yield(); - } - context.releaseOwnership(Robot.drive); - context.startAsync(new setCross()); - log("Finished method!"); - } else { - log("No points!"); - } - } - - /** - * Method which keeps updating how much the robot should turn between rateLimiter calls. - */ - public void updateRotation() { - Robot.drive.setGyro(-Robot.gyro.getGyroYaw()); - driveSettings.setHeading(rotationSpeed(Robot.drive.getCurrentPosition().getHeading(), pointList[targetNum].getHeading())); - Robot.drive.swerveDrive(driveSettings); - } - - /** - * Method which returns whether the circle around the robot intersects the line connecting the two next points. - * @param pointList The list of points the robot is following. - * @return Boolean: whether or not the circle with given radius centered at currentPos intersects with the line between the next two points. - */ - private boolean checkIntersection(Point[] pointList) { - double a; - double b; - double c; - double slope; - if (targetNum < pointList.length - 1) { - slope = pointList[targetNum].slope(pointList[targetNum + 1]); - a = slope * slope + 1; - b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum].getX() + 2 * slope * pointList[targetNum].getY() - 2 * currentPos.getY() * slope; - c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum].getX() * pointList[targetNum].getX() - 2 * slope * pointList[targetNum].getX() * pointList[targetNum].getY() + pointList[targetNum].getY() * pointList[targetNum].getY() + 2 * currentPos.getY() * slope * pointList[targetNum].getX() - 2 * currentPos.getY() * pointList[targetNum].getY() + currentPos.getY() * currentPos.getY() - radius * radius; - if (b * b - 4 * a * c > 0) { - return true; - } - } - return false; - } - - /** - * Method returning which point the robot should move towards. - * @param targetNum The id value of the next point in pointList. - * @param currentPos The current position of the robot. - * @return If the circle around the robot intersects the line connecting the previous and next points, returns whichever intersection point is closest to the next point. Otherwise, returns the next point. - */ - private static Point selectTargetPoint(int targetNum, Point[] pointList) { - double a; - double b; - double c; - double slope; - double potentialX1; - double potentialX2; - double potentialY1; - double potentialY2; - Point potentialPoint1 = new Point(0.0, 0.0); - Point potentialPoint2 = new Point(0.0, 0.0); - if (targetNum == 0) { - return pointList[0]; - } else { - slope = pointList[targetNum - 1].slope(pointList[targetNum]); - a = slope * slope + 1; - b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum - 1].getX() + 2 * slope * pointList[targetNum - 1].getY() - 2 * currentPos.getY() * slope; - c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getX() - 2 * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getY() + pointList[targetNum - 1].getY() * pointList[targetNum - 1].getY() + 2 * currentPos.getY() * slope * pointList[targetNum - 1].getX() - 2 * currentPos.getY() * pointList[targetNum - 1].getY() + currentPos.getY() * currentPos.getY() - radius * radius; - if (b * b - 4 * a * c < 0) { - return pointList[targetNum]; - } else { - potentialX1 = (-1 * b + Math.sqrt(b * b - 4 * a * c))/ (2 * a); - potentialX2 = (-1 * b - Math.sqrt(b * b - 4 * a * c))/ (2 * a); - potentialY1 = slope * (potentialX1 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY(); - potentialY2 = slope * (potentialX2 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY(); - potentialPoint1.set(potentialX1, potentialY1); - potentialPoint2.set(potentialX2, potentialY2); - if (potentialPoint1.distance(pointList[targetNum]) <= potentialPoint2.distance(pointList[targetNum])) { - return potentialPoint1; - } else { - return potentialPoint2; - } - } - } - } - - //Returns if the robot has passed a certain point - /** - * Method returning whether the robot has passed a given point. - * @param P Point which this method determines has been passed. - * @return If the robot is within a certain distance of P, and the distance to P is increasing, returns true. Otherwise, returns false. - */ - private boolean passedPoint(Point P) { - //log(currentPos + " " + P + " " + currentPos.distance(P) + " " + ((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.2) ? " true" : " false")); - //log(((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.4)) ? "true " : "false " + targetNum); - return (currentPos.distance(P) <= 0.03 || (currentPos.distance(P) <= 0.15 && currentPos.distance(P) > lastPos.distance(P) && lastPos.distance(P) > lastPos2.distance(P))); - } - - /** - * Method which returns how much the robot should turn to reach its target heading. - * @param currentRot The current heading of the robot. - * @param targetRot The target heading of the robot. - * @return Returns a value between -1 and 1 corresponding to how much the robot should turn to reach the target point. - */ - private double rotationSpeed(double currentRot, double targetRot) { - double maxSpeed = FollowPointsInputConstants.MAX_ROTATION_SPEED; - double angleDistanceForMaxSpeed = FollowPointsInputConstants.ANGLE_DISTANCE_FOR_MAX_SPEED; - currentRot = mod(currentRot, 360); - targetRot = mod(targetRot, 360); - if (Math.abs(targetRot - currentRot) > Math.abs(targetRot + 360 - currentRot)) { - targetRot += 360; - } - if (Math.abs(targetRot - currentRot) > Math.abs(targetRot - 360 - currentRot)) { - targetRot -= 360; - } - if (Math.abs(targetRot - currentRot) <= angleDistanceForMaxSpeed) { - if (Math.abs(targetRot - currentRot) >= 3) { - return ((currentRot - targetRot) / angleDistanceForMaxSpeed) * maxSpeed; - } - return 0; - } - return maxSpeed * Math.signum(currentRot - targetRot); - } - - //Returns mod(d1, d2), to use to circumvent java's weird % function - private static double mod(double d1, double d2) { - return d1 % d2 + (d1 < 0 ? d2 : 0); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/GyroBalance.java b/src/main/java/com/team766/robot/procedures/GyroBalance.java deleted file mode 100644 index 40b6571a..00000000 --- a/src/main/java/com/team766/robot/procedures/GyroBalance.java +++ /dev/null @@ -1,180 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; -import com.team766.robot.constants.ChargeConstants; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * {@link Procedure} to use the gyro to automatically balance on the charge station. - * See GyroBalance.md for more details - */ -public class GyroBalance extends Procedure { - - // State machine with 4 states for position on ramp - private enum State { - GROUND, - RAMP_TRANSITION, - RAMP_TILT, - RAMP_LEVEL - } - - // Direction determines which direction the robot moves - private enum Direction { - LEFT, - RIGHT, - STOP, - } - - // tilt is the overall combination of pitch and roll - private double tilt = Robot.gyro.getAbsoluteTilt(); - - // absSpeed is unsigned speed value - private double absSpeed; - private State prevState; - private State curState; - private Direction direction; - private final Alliance alliance; - - private final double TOP_TILT = 15.0; - private final double FLAP_TILT = 11; - - // Tweak these values to adjust how the robot balances - private final double LEVEL = 7; - private final double CORRECTION_DELAY = 0.5; - private final double SPEED_GROUND = .3; - private final double SPEED_TRANSITION = .25; - private final double SPEED_TILT = .18; - - /** - * Constructor to create a new GyroBalance instance - * @param alliance Alliance for setting direction towards charge station - */ - public GyroBalance(Alliance alliance) { - this.alliance = alliance; - } - - public void run(Context context) { - context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.drive); - - // curX is current robot x position - double curX = Robot.drive.getCurrentPosition().getX(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - - // driveSpeed is actual value of speed passed into the swerveDrive method - double driveSpeed = 1; - - // Sets movement direction ground state if on ground - setDir(curX); - - // sets starting state if not on ground - if (tilt < LEVEL && curState != State.GROUND) { - curState = State.RAMP_LEVEL; - } else if (tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - } else if (tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - } - - do { - // Sets prevState to the current state and calculates curState - prevState = curState; - curX = Robot.drive.getCurrentPosition().getX(); - tilt = Robot.gyro.getAbsoluteTilt(); - //log("curX:" + curX); - //log("direction: " + direction); - setState(); - - // Both being on Red alliance and needing to move right would make the movement direction negative, so this expression corrects for that - if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) { - driveSpeed = -absSpeed; - } else { - driveSpeed = absSpeed; - } - - // Drives the robot with the calculated speed and direction - Robot.drive.swerveDrive(0, -driveSpeed, 0); - context.yield(); - } - // Loops until robot is level or until a call to the abort() method - while (!(curState == State.RAMP_LEVEL)); - - // After the robot is level, drives for correctionDelay seconds. - // Direction is opposite due to inversion of speed in setState() so it corrects for overshooting - context.waitForSeconds(CORRECTION_DELAY); - - // Locks wheels once balanced - context.startAsync(new setCross()); - - context.releaseOwnership(Robot.drive); - context.releaseOwnership(Robot.gyro); - } - - // Sets state in state machine, see more details in GyroBalance.md - private void setState() { - if (prevState == State.GROUND && tilt > LEVEL) { - curState = State.RAMP_TRANSITION; - absSpeed = SPEED_TRANSITION; - log("Transition, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) { - curState = State.RAMP_TILT; - absSpeed = SPEED_TILT; - log("Tilt, prevState: " + prevState + ", curState: " + curState); - } else if (prevState == State.RAMP_TILT && tilt < LEVEL) { - curState = State.RAMP_LEVEL; - // If level, sets speed to negative to correct for overshooting - absSpeed = -absSpeed; - log("Level, prevState: " + prevState + ", curState: " + curState); - } - if (curState == State.GROUND) { - absSpeed = SPEED_GROUND; - } - } - - /** - * Sets direction towards desired charge station - * If robot is level and outside of charge station boundaries, sets state to ground - * @param curX current robot x position - */ - private void setDir(double curX) { - switch (alliance) { - case Red: - // If to the right of the charge station, go left - if (curX > ChargeConstants.RED_BALANCE_TARGET_X) { - // If level and outside of charge station boundaries, set state to ground - if (tilt < LEVEL && curX > ChargeConstants.RED_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - // If to the left of the charge station, go right - } else { - if (tilt < LEVEL && curX < ChargeConstants.RED_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - case Blue: - // Same logic for blue alliance coordinates - if (curX > ChargeConstants.BLUE_BALANCE_TARGET_X) { - if (tilt < LEVEL && curX > ChargeConstants.BLUE_RIGHT_PT) { - curState = State.GROUND; - } - direction = Direction.LEFT; - } else { - if (tilt < LEVEL && curX < ChargeConstants.BLUE_LEFT_PT) { - curState = State.GROUND; - } - direction = Direction.RIGHT; - } - break; - case Invalid: //drop down - default: - log("Invalid alliance"); - } - } - -} diff --git a/src/main/java/com/team766/robot/procedures/IntakeIn.java b/src/main/java/com/team766/robot/procedures/IntakeIn.java deleted file mode 100644 index b9a18c1b..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeIn.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeIn extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.startIntake(); - Robot.storage.beltIn(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/IntakeOut.java b/src/main/java/com/team766/robot/procedures/IntakeOut.java deleted file mode 100644 index 3a891e7f..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeOut.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeOut extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.reverseIntake(); - Robot.storage.beltOut(); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/IntakeStop.java b/src/main/java/com/team766/robot/procedures/IntakeStop.java deleted file mode 100644 index ca22867b..00000000 --- a/src/main/java/com/team766/robot/procedures/IntakeStop.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class IntakeStop extends Procedure{ - public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OPECHelper.java b/src/main/java/com/team766/robot/procedures/OPECHelper.java deleted file mode 100644 index 141dd256..00000000 --- a/src/main/java/com/team766/robot/procedures/OPECHelper.java +++ /dev/null @@ -1,27 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; -import com.team766.robot.constants.FollowPointsInputConstants; -import java.util.function.BooleanSupplier; -import edu.wpi.first.wpilibj.DriverStation; -import com.team766.odometry.*; - -public class OPECHelper extends Procedure { - - private static final double DIST = 4; - - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - double startX = Robot.drive.getCurrentPosition().getX(); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - new ReverseIntake().run(context); - Robot.drive.swerveDrive(0, FollowPointsInputConstants.SPEED, 0); - context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getX() - startX) > DIST); - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceBalance.java deleted file mode 100644 index df391362..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceBalance extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 2.7)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 2.7)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - - } - new ReverseIntake().run(context); - new GyroBalance(DriverStation.getAlliance()).run(context); - } -} diff --git a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java deleted file mode 100644 index 7e5231c7..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java +++ /dev/null @@ -1,32 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceExitCommunity extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 0.75)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java deleted file mode 100644 index cfc1b60f..00000000 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.odometry.PointDir; -import com.team766.robot.Robot; - -import edu.wpi.first.wpilibj.DriverStation; - -public class OnePieceExitCommunityBalance extends Procedure { - public void run (Context context) { - context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.gyro); - Robot.gyro.resetGyro180(); - Robot.drive.setGyro(Robot.gyro.getGyroYaw()); - switch (DriverStation.getAlliance()) { - case Blue: - Robot.drive.setCurrentPosition(new PointDir(2, 2.7)); - break; - case Red: - Robot.drive.setCurrentPosition(new PointDir(14.5, 2.7)); - break; - case Invalid: //drop down - default: - log("invalid alliance"); - return; - } - log("exiting"); - new OPECHelper().run(context); - log("Transitioning"); - new GyroBalance(DriverStation.getAlliance()).run(context); - } - -} diff --git a/src/main/java/com/team766/robot/procedures/ReverseIntake.java b/src/main/java/com/team766/robot/procedures/ReverseIntake.java deleted file mode 100644 index 53fc329f..00000000 --- a/src/main/java/com/team766/robot/procedures/ReverseIntake.java +++ /dev/null @@ -1,12 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; - -public class ReverseIntake extends Procedure{ - public void run(Context context) { - context.startAsync(new IntakeOut()); - context.waitForSeconds(2); - context.startAsync(new IntakeStop()); - } -} diff --git a/src/main/java/com/team766/robot/procedures/setCross.java b/src/main/java/com/team766/robot/procedures/setCross.java deleted file mode 100644 index e481c027..00000000 --- a/src/main/java/com/team766/robot/procedures/setCross.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.team766.robot.procedures; - -import com.team766.framework.Context; -import com.team766.framework.Procedure; -import com.team766.robot.Robot; - -public class setCross extends Procedure { - - public void run(Context context) { - context.takeOwnership(Robot.drive); - Robot.drive.stopDriveMotors(); - Robot.drive.stopSteerMotors(); - Robot.drive.setCross(); - } - -} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..dad31057 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file