diff --git a/build.gradle b/build.gradle index 293724c3..d3aab25e 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id 'java-library' id 'maven-publish' - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2023.4.3" id 'com.google.protobuf' version '0.8.19' } diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 0da8959d..55b541a2 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -6,14 +6,15 @@ import com.team766.hal.RobotProvider; import com.team766.logging.Category; 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.Intake; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + /** * This class is the glue that binds the controls on the physical operator * interface to the code that allow control of the robot. @@ -32,11 +33,13 @@ public class OI extends Procedure { private double LeftJoystick_Z = 0; private double LeftJoystick_Theta = 0; private boolean isCross = false; - private IntakeState intakeState = IntakeState.IDLE; private static final double HighConeArm1 = 0; private static final double CHA2 = 0; private static final double CMA1 = 0; + + private int state = 0; + private boolean ignoreState = false; // enum generalControl{ // CONE_HIGH_NODE, // CUBE_HIGH_NODE, @@ -65,21 +68,57 @@ public OI() { public void run(Context context) { context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.arms); - context.takeOwnership(Robot.storage); + // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - context.takeOwnership(Robot.grabber); - - // CameraServer.startAutomaticCapture(); - - Robot.arms.resetFirstEncoders(); - Robot.arms.resetSecondEncoders(); + context.takeOwnership(Robot.lights); while (true) { context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); RobotProvider.instance.refreshDriverStationData(); + //TODO: ADD CODE DEPENDING ON WHAT THE STATE SHOULD BE FOR WHICH BUTTONS + if(DriverStation.getMatchTime() < 30 && DriverStation.getMatchTime() > 29){ + Robot.lights.rainbow(); + log("" + DriverStation.getMatchTime()); + ignoreState = true; + } + + if(DriverStation.getMatchTime() <29 && DriverStation.getMatchTime() > 28.7){ + Robot.lights.clearAnimation(); + ignoreState = false; + } + switch (state){ + case 1: + if(ignoreState){ break;} + Robot.lights.signalCube(); + break; + case 2: + if(ignoreState){ break;} + Robot.lights.signalCone(); + break; + case 3: + if(ignoreState){ break;} + Robot.lights.rainbow(); + break; + case 4: + if(ignoreState){ break;} + Robot.lights.hybridScore(); + break; + case 5: + if(ignoreState){ break;} + Robot.lights.midScore(); + break; + case 6: + if(ignoreState){ break;} + Robot.lights.highScore(); + break; + default: + if(!ignoreState){ + Robot.lights.resetLights(); + } + } + + // Add driver controls here - make sure to take/release ownership // of mechanisms when appropriate. @@ -96,6 +135,9 @@ public void run(Context context) { // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); // } + if(DriverStation.isTeleop() && DriverStation.getMatchTime() < 30){ + Robot.lights.signalMalfunction(); + } if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) { Robot.gyro.resetGyro(); @@ -138,43 +180,25 @@ public void run(Context context) { // Sets intake state based on button pressed if (controlPanel.getButtonPressed(InputConstants.INTAKE)) { - if (intakeState == IntakeState.IDLE) { - Robot.intake.startIntake(); - Robot.storage.beltIn(); - intakeState = IntakeState.SPINNINGFWD; - } else { - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); - intakeState = IntakeState.IDLE; - } + // if (Robot.intake.getState() == Intake.State.IDLE) { + // Robot.intake.in(); + // } else { + // Robot.intake.stop(); + //} } - /* if (controlPanel.getButtonPressed(InputConstants.INTAKE_PISTONLESS)){ - if (intakeState == IntakeState.IDLE){ - Robot.intake.intakePistonless(); - Robot.storage.beltIn(); - intakeState = IntakeState.SPINNINGREV; - } else { - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); - intakeState = IntakeState.IDLE; - } - } */ + if (controlPanel.getButtonPressed(InputConstants.OUTTAKE)) { - if (intakeState == IntakeState.IDLE) { - Robot.intake.reverseIntake(); - Robot.storage.beltOut(); - intakeState = IntakeState.SPINNINGREV; - } else { - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); - intakeState = IntakeState.IDLE; - } + // if (Robot.intake.getState() == Intake.State.IDLE) { + // Robot.intake.out(); + // } else { + // Robot.intake.stop(); + // } } // 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()); + context.startAsync(new SetCross()); } isCross = !isCross; } @@ -187,11 +211,10 @@ public void run(Context context) { context.takeOwnership(Robot.drive); // If a button is pressed, drive is just fine adjustment if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) { - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (-leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (-rightJoystickX * FINE_DRIVING_COEFFICIENT)); + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT)); } else { - // On deafault, controls the robot field oriented - // Need negatives here, controls backwards otherwise (most likely specific to CLR) - Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (-leftJoystickX), (leftJoystickY), (-rightJoystickX)); + // On deafault, controls the robot field oriented + Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX), (leftJoystickY), (rightJoystickX)); } } else if (!isCross) { Robot.drive.stopDrive(); @@ -203,37 +226,17 @@ public void run(Context context) { if (controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) { log("Arm cone high"); - Robot.arms.stowed = false; - Robot.arms.pidForArmOne(-17.379); - Robot.arms.pidForArmTwo(-56.61); //previously -66 } if (controlPanel.getButtonPressed(InputConstants.CONE_MID)) { log("Arm cone mid"); - Robot.arms.stowed = false; - Robot.arms.pidForArmOne(2.7765); - Robot.arms.pidForArmTwo(-83.813); //previously -93 } if (controlPanel.getButtonPressed(InputConstants.ARM_READY)) { log("Arm ready"); - Robot.arms.stowed = false; - Robot.arms.pidForArmOne(10.269); - Robot.arms.pidForArmTwo(-90); } if (controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) { - if (Robot.arms.getSecondJointPosition() < -100) { - log("Move arms to Ready position before moving to Pickup"); - } else { - log("Arm pickup"); - Robot.arms.stowed = false; - Robot.arms.pidForArmOne(22.73); - Robot.arms.pidForArmTwo(-62.529); // previously -72.529 - } } if (controlPanel.getButtonPressed(InputConstants.UNSTOWED)) { log("Arm unstowed"); - Robot.arms.stowed = true; - Robot.arms.pidForArmOne(10.269); - Robot.arms.pidForArmTwo(-157.387); } /* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){ Robot.arms.pidForArmOne(22.73); @@ -241,35 +244,25 @@ public void run(Context context) { } */ if (controlPanel.getButton(InputConstants.NUDGE_UP)) { log("Arm nudge up"); - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up()); } if (controlPanel.getButton(InputConstants.NUDGE_DOWN)) { log("Arm nudge down"); - Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down()); } if (controlPanel.getButton(InputConstants.GRAB_IN)) { - Robot.grabber.grabberPickUp(); } else if (rightJoystick.getButton(InputConstants.GRABBER_RELEASE)) { - 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 (controlPanel.getButtonPressed(13)) { - Robot.arms.resetFirstEncoders(); - Robot.arms.resetSecondEncoders(); } } } diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index a9bf6946..9bd0d5aa 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -4,20 +4,16 @@ public class Robot { // Declare mechanisms here - public static Intake intake; - public static Storage storage; + // public static Intake intake; public static Drive drive; - public static Arms arms; public static Gyro gyro; - public static Grabber grabber; + public static Lights lights; public static void robotInit() { // Initialize mechanisms here - intake = new Intake(); - storage = new Storage(); + // intake = new Intake(); drive = new Drive(); - arms = new Arms(); gyro = new Gyro(); - grabber = new Grabber(); + lights = new Lights(); } } diff --git a/src/main/java/com/team766/robot/constants/ConfigConstants.java b/src/main/java/com/team766/robot/constants/ConfigConstants.java new file mode 100644 index 00000000..1a9cfc33 --- /dev/null +++ b/src/main/java/com/team766/robot/constants/ConfigConstants.java @@ -0,0 +1,41 @@ +package com.team766.robot.constants; + +/** Constants used for reading values from the config file. */ +public final class ConfigConstants { + // utility class + private ConfigConstants() {} + + // drive config values + public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight"; + public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft"; + public static final String DRIVE_DRIVE_BACK_RIGHT = "drive.DriveBackRight"; + public static final String DRIVE_DRIVE_BACK_LEFT = "drive.DriveBackLeft"; + + public static final String DRIVE_STEER_FRONT_RIGHT = "drive.SteerFrontRight"; + public static final String DRIVE_STEER_FRONT_LEFT = "drive.SteerFrontLeft"; + public static final String DRIVE_STEER_BACK_RIGHT = "drive.SteerBackRight"; + public static final String DRIVE_STEER_BACK_LEFT = "drive.SteerBackLeft"; + + // intake config values + public static final String INTAKE_MOTOR = "intake.motor"; + + // wrist config values + public static final String WRIST_MOTOR = "wrist.motor"; + public static final String WRIST_PGAIN = "wrist.pGain"; + public static final String WRIST_IGAIN = "wrist.iGain"; + public static final String WRIST_DGAIN = "wrist.dGain"; + public static final String WRIST_FFGAIN = "wrist.ffGain"; + + // elevator config values + public static final String ELEVATOR_LEFT_MOTOR = "elevator.leftMotor"; + public static final String ELEVATOR_RIGHT_MOTOR = "elevator.rightMotor"; + public static final String ELEVATOR_PGAIN = "elevator.pGain"; + public static final String ELEVATOR_IGAIN = "elevator.iGain"; + public static final String ELEVATOR_DGAIN = "elevator.dGain"; + public static final String ELEVATOR_FFGAIN = "elevator.ffGain"; + public static final String ELEVATOR_MAX_VELOCITY = "elevator.maxVelocity"; + public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.minOutputVelocity"; + public static final String ELEVATOR_MAX_ACCEL = "elevator.maxAccel"; +} + + diff --git a/src/main/java/com/team766/robot/constants/OdometryInputConstants.java b/src/main/java/com/team766/robot/constants/OdometryInputConstants.java index f65ad485..ddf4a25a 100644 --- a/src/main/java/com/team766/robot/constants/OdometryInputConstants.java +++ b/src/main/java/com/team766/robot/constants/OdometryInputConstants.java @@ -14,8 +14,8 @@ public final class OdometryInputConstants { public static final double GEAR_RATIO = 6.75; //Unique to the type of swerve module we have. For every revolution of the wheel, the encoders will increase by 2048. public static final int ENCODER_TO_REVOLUTION_CONSTANT = 2048; - //The distance between the centers of the wheels on each side of the robot. This was measured as 22 inches, then converted to meters. - public static final double DISTANCE_BETWEEN_WHEELS = 22 * 2.54 / 100; + //The distance between the centers of the wheels on each side of the robot. This was measured as 20.5 inches, then converted to meters. + public static final double DISTANCE_BETWEEN_WHEELS = 20.5 * 2.54 / 100; //How often odometry updates, in seconds. public static final double RATE_LIMITER_TIME = 0.05; 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 a7cb1e57..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ /dev/null @@ -1,425 +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; - - // 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` - private static final double altEncoder1Offset = 0.25; - private static final double altEncoder2Offset = 0.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; - public boolean stowed = false; - - ArmState theStateOf1 = ArmState.ANTIGRAV; - ArmState theStateOf2 = ArmState.ANTIGRAV; - - - 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.64); // TODO: these need tweaking from altEncoder1Offset - altEncoder2.setZeroOffset(0.19); //Old values: 0.807, 0.446 - altEncoder2.setPositionConversionFactor(0.975); // 0.89 - - 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); - } - - public double getSecondJointPosition() { - return ArmsUtil.EUTodegrees(secondJoint.getSensorPosition()); - } - - // Resets encoders - public void resetFirstEncoders() { - checkContextOwnership(); - - // altEncoder1Offset = what is the value of altEncoder1 when firstJoint is vertical - double firstJointAbsEncoder = Math.IEEEremainder(altEncoder1.getPosition() - altEncoder1Offset, 1.0); - double firstJointRelEncoder = ArmsUtil.AbsToEU(firstJointAbsEncoder); - - // set the sensor positions and setpoint of our rel encoders - firstJoint.setSensorPosition(firstJointRelEncoder); - log("Reset Encoder 1 (in degrees): "+ArmsUtil.EUTodegrees(firstJointRelEncoder)); - firstJointPosition = ArmsUtil.EUTodegrees(firstJointRelEncoder); - } - - public void resetSecondEncoders() { - checkContextOwnership(); - - // 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 = - Math.IEEEremainder(altEncoder1.getPosition() - altEncoder1Offset, 1.0); - double secondJointAbsEncoder = - Math.IEEEremainder(altEncoder2.getPosition() - altEncoder2Offset, 1.0); - double secondJointRelEncoder = ArmsUtil.AbsToEU( - firstJointAbsEncoder + secondJointAbsEncoder); - - // set the sensor positions and setpoint of our rel encoders - secondJoint.setSensorPosition(secondJointRelEncoder); - log("Reset Encoder 2 (in degrees): "+ArmsUtil.EUTodegrees(secondJointRelEncoder)); - secondJointPosition = ArmsUtil.EUTodegrees(secondJointRelEncoder); - } - - public void armStop(){ - brake(); - 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: " + altEncoder1.getPosition()); - // log("AE2: " + altEncoder2.getPosition()); - SmartDashboard.putNumber("Joint 1 Motor Encoder", ArmsUtil.EUTodegrees(firstJoint.getSensorPosition())); - SmartDashboard.putNumber("Joint 2 Motor Encoder", ArmsUtil.EUTodegrees(secondJoint.getSensorPosition())); - SmartDashboard.putNumber("Joint 1 Abs Encoder", 360 * Math.IEEEremainder(altEncoder1.getPosition() - altEncoder1Offset, 1.0)); - SmartDashboard.putNumber("Joint 2 Abs Encoder", 360 * Math.IEEEremainder(altEncoder2.getPosition() - altEncoder2Offset, 1.0)); - - } - - @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); - } - - logs(); - - // log("First Joint AntiGrav: "+getAntiGravFirstJoint()); - // log("Second Joint AntiGrav: "+getAntiGravSecondJoint()); - switch(theStateOf1) { - case OFF: - break; - case ANTIGRAV: - if (stowed){ - firstJoint.set(0); - } else { - 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 >= 15){ - 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: - if (stowed){ - secondJoint.set(0); //This will activate brake mode - } else { - 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 >= 15){ - 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())); - - // update shuffleboard periodically - // SmartDashboard.putNumber("Alt Encoder 1", altEncoder1.getPosition()); - // SmartDashboard.putNumber("Alt Encoder 2", altEncoder2.getPosition()); - } -} - -/* ~~ 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 0dd4a4f5..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ArmsUtil.java +++ /dev/null @@ -1,56 +0,0 @@ -package com.team766.robot.mechanisms; - -/** - * Helper classes - */ -public final class ArmsUtil { - - /** - * Static class only - */ - private ArmsUtil() { - } - - public static double degreesToEU(double angle) { - // (chain reduction) * (planetary reduction) * (degrees to rotations) - return (33. / 10.) * (75./1.) * (1./360.) * angle; - } - - public static double EUTodegrees(double EU) { - // (chain reduction) * (planetary reduction) * (rotations to degrees) - return (10. / 33.) * (1./75.) * (360./1.) * EU; - } - - 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 ed73b2a1..25d4b91e 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -6,6 +6,7 @@ import com.team766.hal.RobotProvider; import com.team766.hal.MotorController.ControlMode; import com.team766.logging.Category; +import static com.team766.robot.constants.ConfigConstants.*; import com.team766.robot.constants.SwerveDriveConstants; import com.team766.robot.constants.OdometryInputConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -68,24 +69,22 @@ public Drive() { // Initializations of motors // Initialize the drive motors - m_DriveFR = RobotProvider.instance.getMotor("drive.DriveFrontRight"); - m_DriveFL = RobotProvider.instance.getMotor("drive.DriveFrontLeft"); - m_DriveBR = RobotProvider.instance.getMotor("drive.DriveBackRight"); - m_DriveBL = RobotProvider.instance.getMotor("drive.DriveBackLeft"); + m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); // Initialize the steering motors - m_SteerFR = RobotProvider.instance.getMotor("drive.SteerFrontRight"); - m_SteerFL = RobotProvider.instance.getMotor("drive.SteerFrontLeft"); - m_SteerBR = RobotProvider.instance.getMotor("drive.SteerBackRight"); - m_SteerBL = RobotProvider.instance.getMotor("drive.SteerBackLeft"); - - + m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); // Initialize the encoders - e_FrontRight = new CANCoder(22, SwerveDriveConstants.SWERVE_CANBUS); - e_FrontLeft = new CANCoder(23, SwerveDriveConstants.SWERVE_CANBUS); - e_BackRight = new CANCoder(21, SwerveDriveConstants.SWERVE_CANBUS); - e_BackLeft = new CANCoder(24, SwerveDriveConstants.SWERVE_CANBUS); + e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); // Current limit for motors to avoid breaker problems m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); @@ -163,10 +162,10 @@ public void controlModuleSteerAndPower(MotorController driveMotor, MotorControll public void controlRobotOriented(double x, double y, double turn) { // Finds the vectors for turning and for translation of each module, and adds them // Applies this for each module - controlModuleSteerAndPower(m_DriveFL, m_SteerFL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FL_X, SwerveDriveConstants.FL_Y).normalize()), offsetFL); - controlModuleSteerAndPower(m_DriveFR, m_SteerFR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FR_X, SwerveDriveConstants.FR_Y).normalize()), offsetFR); - controlModuleSteerAndPower(m_DriveBR, m_SteerBR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BR_X, SwerveDriveConstants.BR_Y).normalize()), offsetBR); - controlModuleSteerAndPower(m_DriveBL, m_SteerBL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BL_X, SwerveDriveConstants.BL_Y).normalize()), offsetBL); + controlModuleSteerAndPower(m_DriveFL, m_SteerFL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize()), offsetFL); + controlModuleSteerAndPower(m_DriveFR, m_SteerFR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize()), offsetFR); + controlModuleSteerAndPower(m_DriveBR, m_SteerBR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize()), offsetBR); + controlModuleSteerAndPower(m_DriveBL, m_SteerBL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize()), offsetBL); } /** @@ -179,7 +178,8 @@ public void controlRobotOriented(double x, double y, double turn) { public void controlFieldOriented(double yawRad, double x, double y, double turn) { // Applies a rotational translation to controlRobotOriented // Counteracts the forward direction changing when the robot turns - controlRobotOriented(Math.cos(yawRad) * x - Math.sin(yawRad) * y, Math.cos(yawRad) * y + Math.sin(yawRad) * x, turn); + // TODO: change to inverse rotation matrix (rather than negative angle) + controlRobotOriented(Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, turn); } /* diff --git a/src/main/java/com/team766/robot/mechanisms/Elevator.java b/src/main/java/com/team766/robot/mechanisms/Elevator.java new file mode 100644 index 00000000..510d290a --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Elevator.java @@ -0,0 +1,134 @@ +package com.team766.robot.mechanisms; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.SparkMaxPIDController.AccelStrategy; +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.Severity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import static com.team766.robot.constants.ConfigConstants.*; + +public class Elevator extends Mechanism { + public enum Position { + + // TODO: do we need separate heights for cones vs cubes? + + /** Elevator is at lowest (start) position. */ + LOW(0), + /** Elevator is the appropriate height to place game pieces at the mid node. */ + MID(45), + /** Elevator is at appropriate height to place game pieces at the high node. */ + HIGH(180), + /** Elevator is at appropriate height to grab cubes from the human player. */ + HUMAN_CUBES(200), + /** Elevator is at appropriate height to grab cones from the human player. */ + HUMAN_CONES(200); + + private final int height; + + Position(int position) { + this.height = position; + } + + private int getHeight() { + return height; + } + } + + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider maxAccel; + + + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Elevator. + */ + public Elevator() { + MotorController halLeftMotor = RobotProvider.instance.getMotor(ELEVATOR_LEFT_MOTOR); + MotorController halRightMotor = RobotProvider.instance.getMotor(ELEVATOR_RIGHT_MOTOR); + + if (!((halLeftMotor instanceof CANSparkMax)&&(halRightMotor instanceof CANSparkMax))) { + log(Severity.ERROR, "Motors are not CANSparkMaxes!"); + throw new IllegalStateException("Motor are not CANSparkMaxes!"); + } + + leftMotor = (CANSparkMax) halLeftMotor; + rightMotor = (CANSparkMax) halRightMotor; + + rightMotor.follow(leftMotor); + + pidController = leftMotor.getPIDController(); + pidController.setFeedbackDevice(leftMotor.getEncoder()); + + pGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(ELEVATOR_FFGAIN); + maxVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_VELOCITY); + minOutputVelocity = ConfigFileReader.getInstance().getDouble(ELEVATOR_MIN_OUTPUT_VELOCITY); + maxAccel = ConfigFileReader.getInstance().getDouble(ELEVATOR_MAX_ACCEL); + } + + /** + * Returns the current height of the elevator, in inches ('Murica). + */ + public double getHeight() { + return EncoderUtils.elevatorEUToHeight(leftMotor.getEncoder().getPosition()); + } + + /** + * Moves the elevator to a pre-set {@link Position}. + */ + public void moveTo(Position position) { + moveTo(position.getHeight()); + } + + /** + * Moves the elevator to a specific position (in inches). + */ + public void moveTo(int position) { + checkContextOwnership(); + + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + pidController.setFF(ffGain.get()); + + pidController.setOutputRange(-1, 1); + + pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0); + pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0); + pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0); + pidController.setSmartMotionMaxAccel(maxAccel.get(), 0); + + // TODO: do we need to set output range? + + // convert the desired target degrees to encoder units + double encoderUnits = EncoderUtils.elevatorHeightToEU(position); + + // set the reference point for the wrist + pidController.setReference(encoderUnits, ControlType.kSmartMotion); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("ElevatorHeight", getHeight()); + } + } +} diff --git a/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java new file mode 100644 index 00000000..dcc074fa --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java @@ -0,0 +1,73 @@ +package com.team766.robot.mechanisms; + +/** + * Utility class to convert between encoder units and physical units we use for different + * mechanisms. + */ +public final class EncoderUtils { + + /** + * Utility class. + */ + private EncoderUtils() { + } + + /** + * Converts a target rotation (in degrees) to encoder units for the wrist motor. + */ + public static double wristDegreesToEU(double angle) { + // (chain reduction) * (planetary reduction) * (degrees to rotations) + // return (33. / 10.) * (75./1.) * (1./360.) * angle; + return (0. / 1.) * (0. / 1.) * (1. / 360.) * angle; + } + + /** + * Converts the wrist motor's encoder units to degrees. + */ + public static double wristEUTodegrees(double eu) { + // (chain reduction) * (planetary reduction) * (rotations to degrees) + // return (10. / 33.) * (1. / 75.) * (360. / 1.) * eu; + return (0. / 1.) * (0./1.) * (360./1.) * eu; + } + + /** + * Converts a desired height (in inches) to encoder units for the elevator motors. + */ + public static double elevatorHeightToEU(double position) { + // STOPSHIP: fix this. + return 0.; + } + + /** + * Converts the elevator motor's encoder units to a height (in inches). + */ + public static double elevatorEUToHeight(double eu) { + // STOPSHIP: fix this. + return 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/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/Gyro.java b/src/main/java/com/team766/robot/mechanisms/Gyro.java index e11662cf..3804ec20 100644 --- a/src/main/java/com/team766/robot/mechanisms/Gyro.java +++ b/src/main/java/com/team766/robot/mechanisms/Gyro.java @@ -1,22 +1,13 @@ package com.team766.robot.mechanisms; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.I2C.Port; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.ctre.phoenix.sensors.Pigeon2; import com.team766.framework.Mechanism; -import com.team766.hal.EncoderReader; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; import com.team766.library.RateLimiter; -import com.team766.hal.MotorController; import com.team766.logging.Category; -//import edu.wpi.first.wpilibj.I2C.Port; -//import com.team766.hal.GyroReader; -//import com.kauailabs.navx.frc.*; -import com.ctre.phoenix.sensors.Pigeon2; +import com.team766.robot.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Gyro extends Mechanism { - Pigeon2 g_gyro = new Pigeon2(0, "Swervavore"); + Pigeon2 g_gyro = new Pigeon2(0, SwerveDriveConstants.SWERVE_CANBUS); double[] gyroArray = new double[3]; private RateLimiter l_loggingRate = new RateLimiter(0.05); public Gyro() { diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java index 3ce68507..a0b67af1 100644 --- a/src/main/java/com/team766/robot/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/mechanisms/Intake.java @@ -3,82 +3,69 @@ 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 com.team766.library.RateLimiter; - -import edu.wpi.first.wpilibj.PneumaticHub; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - +import static com.team766.robot.constants.ConfigConstants.*; + +/** + * Basic intake. Mounted on end of {@link Wrist}. The intake can be controlled to attempt to + * pull a game piece in via {@link #in}, release a contained game piece via {@link #out}, or stop + * moving via {@link #stop}. + * + * Note: the Intake does not maintain any state as to whether or not it contains a game piece. + */ public class Intake extends Mechanism { + + /** + * The current movement state for the intake. + */ + public enum State { + IDLE, + IN, + OUT + } - private MotorController bottomWheels; - private MotorController topBelt; - private SolenoidController leftPiston; - private SolenoidController rightPiston; - private PneumaticHub ph; - private RateLimiter runRateLimiter = new RateLimiter(0.5); + private MotorController motor; + private State state = State.IDLE; + /** + * Constructs a new Intake. + */ 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); - + motor = RobotProvider.instance.getMotor(INTAKE_MOTOR); } - public void intakePistonless() { - topBelt.set(1.0); - bottomWheels.set(1.0); + /** + * Returns the current movement state of the intake. + * + * @return The current movement state of the intake. + */ + public State getState() { + return state; } - public void stopIntake() { + /** + * Turns the intake motor on in order to pull a game piece into the mechanism. + */ + public void in() { checkContextOwnership(); - - topBelt.set(0.0); - bottomWheels.set(0.0); - pistonsIn(); + motor.set(1.0); + state = State.IN; } - public void pistonsOut() { + /** + * Turns the intake motor on in reverse direction, to release any contained game piece. + */ + public void out() { checkContextOwnership(); - log("pistons out"); - - leftPiston.set(true); - rightPiston.set(true); - } - - public void pistonsIn() { - checkContextOwnership(); - - leftPiston.set(false); - rightPiston.set(false); + motor.set(-1.0); + state = State.OUT; } - public void reverseIntake() { + /** + * Turns off the intake motor. + */ + public void stop() { checkContextOwnership(); - - topBelt.set(-1.0); - bottomWheels.set(-1.0); - - pistonsIn(); - } - - public void run(){ - if(!runRateLimiter.next()) return; - - SmartDashboard.putNumber("Storage PSI",ph.getPressure(0)); + motor.set(0.0); + state = State.IDLE; } } \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Lights.java b/src/main/java/com/team766/robot/mechanisms/Lights.java new file mode 100644 index 00000000..1c0af62f --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Lights.java @@ -0,0 +1,185 @@ +package com.team766.robot.mechanisms; +import org.apache.commons.math3.analysis.function.Ceil; +import org.apache.commons.math3.stat.correlation.StorelessCovariance; +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.StrobeAnimation; +import com.ctre.phoenix.led.TwinkleAnimation; +import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; +import com.team766.framework.Mechanism; +import edu.wpi.first.wpilibj.DriverStation; + + +public class Lights extends Mechanism{ + + private CANdle candle; + private static final int CANID = 5; + private int numLEDs = 42; + RainbowAnimation rainbowAnim = new RainbowAnimation(1, 3, numLEDs); + + int curAnimation = -1; + + public Lights(){ + candle = new CANdle(CANID); + } + + public void setNumLEDs(int num){ + checkContextOwnership(); + numLEDs = num; + rainbowAnim.setNumLed(num); + } + + public void signalCube(){ + checkContextOwnership(); + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(142, 38, 252); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(142, 38, 252); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(142, 38, 252); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + + public void resetLights(){ + checkContextOwnership(); + candle.setLEDs(255, 255, 255); + } + + public void signalCone(){ + checkContextOwnership(); + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(250, 196, 32); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(250, 196, 32); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(250, 196, 32); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + + } + + public void auton(){ + candle.setLEDs(0, 223, 247); + } + + public void signalMalfunction(){ + checkContextOwnership(); + candle.setLEDs(255, 0, 0); + } + + public void rainbow(){ + checkContextOwnership(); + candle.clearAnimation(curAnimation); + candle.animate(rainbowAnim,0); + curAnimation = 0; + + } + + public void clearAnimation(){ + checkContextOwnership(); + if(curAnimation != -1){ + candle.clearAnimation(curAnimation); + curAnimation = -1; + } + + + + } + + public void hybridScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(165, 128, 65); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(165, 128, 65); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(165, 128, 65); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + + //color of justin and kapils shirts + public void midScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(81,102,52); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(81,102,52); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(81,102,52); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + + public void highScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(202, 39, 75); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(202, 39, 75); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(202, 39, 75); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + +} diff --git a/src/main/java/com/team766/robot/mechanisms/OldDrive.java b/src/main/java/com/team766/robot/mechanisms/OldDrive.java deleted file mode 100644 index ef549d0a..00000000 --- a/src/main/java/com/team766/robot/mechanisms/OldDrive.java +++ /dev/null @@ -1,676 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.EncoderReader; -import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController; -import com.team766.hal.MotorController.ControlMode; -import com.team766.hal.simulator.Encoder; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import com.team766.library.ValueProvider; -import com.team766.logging.Category; -import com.team766.simulator.ProgramInterface.RobotMode; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import com.team766.config.ConfigFileReader; -import com.team766.framework.Mechanism; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderConfiguration; -import com.team766.odometry.Odometry; -import com.team766.odometry.Point; -import com.team766.odometry.PointDir; -import com.team766.hal.MotorControllerCommandFailedException; -import com.team766.robot.constants.*; - -public class OldDrive extends Mechanism { - - private MotorController m_DriveFrontRight; - private MotorController m_DriveFrontLeft; - private MotorController m_DriveBackRight; - private MotorController m_DriveBackLeft; - - private MotorController m_SteerFrontRight; - private MotorController m_SteerFrontLeft; - private MotorController m_SteerBackRight; - private MotorController m_SteerBackLeft; - - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - private ValueProvider drivePower; - - private double gyroValue; - - private static PointDir currentPosition; - - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - private Odometry swerveOdometry; - - public OldDrive() { - - loggerCategory = Category.DRIVE; - // Initializations of motors - // Initialize the drive motors - m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight"); - m_DriveFrontLeft = RobotProvider.instance.getMotor("drive.DriveFrontLeft"); - m_DriveBackRight = RobotProvider.instance.getMotor("drive.DriveBackRight"); - m_DriveBackLeft = RobotProvider.instance.getMotor("drive.DriveBackLeft"); - // Initialize the steering motors - m_SteerFrontRight = RobotProvider.instance.getMotor("drive.SteerFrontRight"); - m_SteerFrontLeft = RobotProvider.instance.getMotor("drive.SteerFrontLeft"); - m_SteerBackRight = RobotProvider.instance.getMotor("drive.SteerBackRight"); - m_SteerBackLeft = RobotProvider.instance.getMotor("drive.SteerBackLeft"); - - // Setting up the "config" - CANCoderConfiguration config = new CANCoderConfiguration(); - config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; - // The encoders output "encoder" values, so we need to convert that to degrees (because that - // is what the cool kids are using) - config.sensorCoefficient = 360.0 / 4096.0; - // The offset is going to be changed in ctre, but we can change it here too. - // config.magnetOffsetDegrees = Math.toDegrees(configuration.getOffset()); - config.sensorDirection = true; - - // initialize the encoders - e_FrontRight = new CANCoder(22, "Swervavore"); - // e_FrontRight.configAllSettings(config, 250); - e_FrontLeft = new CANCoder(23, "Swervavore"); - // e_FrontLeft.configAllSettings(config, 250); - e_BackRight = new CANCoder(21, "Swervavore"); - // e_BackRight.configAllSettings(config, 250); - e_BackLeft = new CANCoder(24, "Swervavore"); - // e_BackLeft.configAllSettings(config, 250); - - - // Current limit for motors to avoid breaker problems (mostly to avoid getting electrical - // people to yell at us) - m_DriveFrontRight.setCurrentLimit(35); - m_DriveFrontLeft.setCurrentLimit(35); - m_DriveBackRight.setCurrentLimit(35); - m_DriveBackLeft.setCurrentLimit(35); - m_DriveBackLeft.setInverted(true); - m_DriveBackRight.setInverted(true); - m_SteerFrontRight.setCurrentLimit(30); - m_SteerFrontLeft.setCurrentLimit(30); - m_SteerBackRight.setCurrentLimit(30); - m_SteerBackLeft.setCurrentLimit(30); - - // Setting up the connection between steering motors and cancoders - // m_SteerFrontRight.setRemoteFeedbackSensor(e_FrontRight, 0); - // m_SteerFrontLeft.setRemoteFeedbackSensor(e_FrontLeft, 0); - // m_SteerBackRight.setRemoteFeedbackSensor(e_BackRight, 0); - // m_SteerBackLeft.setRemoteFeedbackSensor(e_BackLeft, 0); - - m_SteerFrontRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerFrontLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerBackRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - m_SteerBackLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - configPID(); - - // Sets up odometry - currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFrontRight, m_DriveFrontLeft, m_DriveBackLeft, - m_DriveBackRight}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = - new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; - log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); - swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - setFrontRightEncoders(); - setFrontLeftEncoders(); - setBackRightEncoders(); - setBackLeftEncoders(); - } - - // A set of simple functions for the sake of adding vectors - /** - * Returns the pythagorean theorem of two numbers - * - * @param x First number - * @param y Second number - * @return - */ - public double pythagorean(double x, double y) { - return Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); - } - - /** - * Returns the angle of a vector, used by joystick inputs - * - * @param LR The x value of the vector - * @param FB The y value of the vector - * @return The angle of the vector - */ - public double getAngle(double LR, double FB) { - return Math.toDegrees(Math.atan2(LR, -FB)); - } - - /** - * Returns whether two angles are within 90 degrees of each other, used to see if the wheels - * should move backwards or not - * - * @param angle1 The first angle - * @param angle2 The second angle - * @return If they are within 90 degrees of each other - */ - public boolean withinHalfACircle(double angle1, double angle2) { - angle1 = mod(angle1, 360); - angle2 = mod(angle2, 360); - if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) { - angle2 += 360; - } - if (Math.abs(angle2 - angle1) > Math.abs(angle2 - 360 - angle1)) { - angle2 -= 360; - } - return Math.abs(angle2 - angle1) <= 90; - } - - // 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); - } - - /** - * Rounds a number based on its value and places - * - * @param value The number to be rounded - * @param places The number of places to round to - * @return The rounded number - */ - public double round(double value, int places) { - double scale = Math.pow(10, places); - return Math.round(value * scale) / scale; - } - - /** - * Adds two vectors together - * - * @param FirstMag The magnitude of the first vector - * @param FirstAng The angle of the first vector - * @param SecondMag The magnitude of the second vector - * @param SecondAng The angle of the second vector - * @return New angle of the vector - */ - public double NewAng(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { - double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) - + SecondMag * Math.cos(Math.toRadians(SecondAng)); - double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) - + SecondMag * Math.sin(Math.toRadians(SecondAng)); - return round(Math.toDegrees(Math.atan2(FinalY, FinalX)), 5); - } - - /** - * Adds two vectors together - * - * @param FirstMag The magnitude of the first vector - * @param FirstAng The angle of the first vector - * @param SecondMag The magnitude of the second vector - * @param SecondAng The angle of the second vector - * @return New magnitude of the vector - */ - public double NewMag(double FirstMag, double FirstAng, double SecondMag, double SecondAng) { - double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng)) - + SecondMag * Math.cos(Math.toRadians(SecondAng)); - double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng)) - + SecondMag * Math.sin(Math.toRadians(SecondAng)); - return round(Math.sqrt(Math.pow(FinalX, 2) + Math.pow(FinalY, 2)), 5); - } - - /** - * Corrects the joystick inputs to make them more accurate, currently unused - * - * @param Joystick The joystick value to be corrected - * @returnThe corrected joystick value - */ - public static double correctedJoysticks(double Joystick) { - if (Joystick >= 0) { - return (3.0 * Math.pow(Joystick, 2) - 2.0 * Math.pow(Joystick, 3)); - } else { - return (-1 * 3.0 * Math.pow(-1 * Joystick, 2) + 2.0 * Math.pow(-1 * Joystick, 3)); - } - } - - /** - * Converts the angle of the joystick to the angle of the robot compared to the field by using - * the gyro - * - * @param angle The angle of the joystick - * @param gyro The angle of the gyro - * @return The angle of the robot compared to the field - */ - public static double fieldAngle(double angle, double gyro) { - double newAngle; - newAngle = angle - gyro; - if (newAngle < 0) { - newAngle = newAngle + 360; - } - if (newAngle >= 180) { - newAngle = newAngle - 360; - } - return newAngle; - } - - /** - * Method to correct angles for the falcon encoders - * - * @param newAngle The given angle value - * @param lastAngle The last angle value - * @return The corrected angle value - */ - public static double newAngle(double newAngle, double lastAngle) { - while (newAngle < 0) - newAngle += 360; - while (newAngle < (lastAngle - 180)) - newAngle += 360; - while (newAngle > (lastAngle + 180)) - newAngle -= 360; - return newAngle; - } - - /** - * Sets the gyro value, used to switch between field and robot orientation - * - * @param value The value to set the gyro to - */ - public void setGyro(double value) { - gyroValue = value; - } - - /** - * This method is used to drive the robot in 2D without turning, using the joystick values. - * - * @param JoystickX The x value of the joystick - * @param JoystickY The y value of the joystick - */ - public void drive2D(double JoystickX, double JoystickY) { - checkContextOwnership(); - // double power = pythagorean((JoystickX), correctedJoysticks(JoystickY))/Math.sqrt(2); - double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); - double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); - - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontRight))) { - m_DriveFrontRight.set(power); - setFrontRightAngle(newAngle(angle, getCurrentAngle(m_SteerFrontRight))); - } else { - m_DriveFrontRight.set(-power); - setFrontRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontRight))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontLeft))) { - m_DriveFrontLeft.set(power); - setFrontLeftAngle(newAngle(angle, getCurrentAngle(m_SteerFrontLeft))); - } else { - m_DriveFrontLeft.set(-power); - setFrontLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontLeft))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackRight))) { - m_DriveBackRight.set(power); - setBackRightAngle(newAngle(angle, getCurrentAngle(m_SteerBackRight))); - } else { - m_DriveBackRight.set(-power); - setBackRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackRight))); - } - - if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackLeft))) { - m_DriveBackLeft.set(power); - setBackLeftAngle(newAngle(angle, getCurrentAngle(m_SteerBackLeft))); - } else { - m_DriveBackLeft.set(-power); - setBackLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackLeft))); - } - } - - /** - * This method is used to drive the robot in 2D without turning, using a point. - * - * @param joystick The point to use for the joystick values - */ - public void drive2D(Point joystick) { - drive2D(joystick.getX(), joystick.getY()); - } - - /** - * This method stops all of the drive motors - */ - public void stopDriveMotors() { - checkContextOwnership(); - m_DriveFrontRight.stopMotor(); - m_DriveFrontLeft.stopMotor(); - m_DriveBackRight.stopMotor(); - m_DriveBackLeft.stopMotor(); - } - - /** - * This method stops all of the steer motors - */ - public void stopSteerMotors() { - checkContextOwnership(); - m_SteerFrontRight.stopMotor(); - m_SteerFrontLeft.stopMotor(); - m_SteerBackRight.stopMotor(); - m_SteerBackLeft.stopMotor(); - } - - /** - * This method is the main method for driving the robot, using the joystick values. - * - * @param JoystickX The x value of the joystick - * @param JoystickY The y value of the joystick - * @param JoystickTheta The theta value of the joystick (for turning) - */ - public void swerveDrive(double JoystickX, double JoystickY, double JoystickTheta) { - checkContextOwnership(); - double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY)); - double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue); - double frPower; - double flPower; - double brPower; - double blPower; - double frAngle; - double flAngle; - double brAngle; - double blAngle; - if (JoystickTheta >= 0) { - frPower = NewMag(power, angle, JoystickTheta, 45); - flPower = NewMag(power, angle, JoystickTheta, -45); - brPower = NewMag(power, angle, JoystickTheta, 135); - blPower = NewMag(power, angle, JoystickTheta, -135); - frAngle = NewAng(power, angle, JoystickTheta, 45); - flAngle = NewAng(power, angle, JoystickTheta, 135); - brAngle = NewAng(power, angle, JoystickTheta, -45); - blAngle = NewAng(power, angle, JoystickTheta, -135); - } else { - frPower = NewMag(power, angle, Math.abs(JoystickTheta), -135); - flPower = NewMag(power, angle, Math.abs(JoystickTheta), 135); - brPower = NewMag(power, angle, Math.abs(JoystickTheta), -45); - blPower = NewMag(power, angle, Math.abs(JoystickTheta), 45); - frAngle = NewAng(power, angle, Math.abs(JoystickTheta), -135); - flAngle = NewAng(power, angle, Math.abs(JoystickTheta), -45); - brAngle = NewAng(power, angle, Math.abs(JoystickTheta), 135); - blAngle = NewAng(power, angle, Math.abs(JoystickTheta), 45); - } - if (Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)) > 1) { - frPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - flPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - brPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - blPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)); - } - - if (withinHalfACircle(frAngle, getCurrentAngle(m_SteerFrontRight))) { - m_DriveFrontRight.set(frPower); - setFrontRightAngle(newAngle(frAngle, getCurrentAngle(m_SteerFrontRight))); - } else { - m_DriveFrontRight.set(-frPower); - setFrontRightAngle(newAngle(180 + frAngle, getCurrentAngle(m_SteerFrontRight))); - } - - if (withinHalfACircle(flAngle, getCurrentAngle(m_SteerFrontLeft))) { - m_DriveFrontLeft.set(flPower); - setFrontLeftAngle(newAngle(flAngle, getCurrentAngle(m_SteerFrontLeft))); - } else { - m_DriveFrontLeft.set(-flPower); - setFrontLeftAngle(newAngle(180 + flAngle, getCurrentAngle(m_SteerFrontLeft))); - } - - if (withinHalfACircle(brAngle, getCurrentAngle(m_SteerBackRight))) { - m_DriveBackRight.set(brPower); - setBackRightAngle(newAngle(brAngle, getCurrentAngle(m_SteerBackRight))); - } else { - m_DriveBackRight.set(-brPower); - setBackRightAngle(newAngle(180 + brAngle, getCurrentAngle(m_SteerBackRight))); - } - - if (withinHalfACircle(blAngle, getCurrentAngle(m_SteerBackLeft))) { - m_DriveBackLeft.set(blPower); - setBackLeftAngle(newAngle(blAngle, getCurrentAngle(m_SteerBackLeft))); - } else { - m_DriveBackLeft.set(-blPower); - setBackLeftAngle(newAngle(180 + blAngle, getCurrentAngle(m_SteerBackLeft))); - } - } - - /** - * This method is used to drive the robot with swerve using a PointDir - * - * @param joystick The PointDir to use for the joystick values - */ - 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 - * - * @param Joystick The joystick value to use for turning - */ - public void turning(double Joystick) { - checkContextOwnership(); - if (Joystick > 0) { - setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); - setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); - setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); - setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); - m_DriveFrontRight.set(Math.abs(Joystick)); - m_DriveFrontLeft.set(Math.abs(Joystick)); - m_DriveBackRight.set(Math.abs(Joystick)); - m_DriveBackLeft.set(Math.abs(Joystick)); - } - if (Joystick < 0) { - setFrontRightAngle(newAngle(-45, getCurrentAngle(m_SteerFrontRight))); - setFrontLeftAngle(newAngle(-135, getCurrentAngle(m_SteerFrontLeft))); - setBackRightAngle(newAngle(45, getCurrentAngle(m_SteerBackRight))); - setBackLeftAngle(newAngle(135, getCurrentAngle(m_SteerBackLeft))); - m_DriveFrontRight.set(Math.abs(Joystick)); - m_DriveFrontLeft.set(Math.abs(Joystick)); - m_DriveBackRight.set(Math.abs(Joystick)); - m_DriveBackLeft.set(Math.abs(Joystick)); - } - } - - private double getCurrentAngle(MotorController motor) { - return Math.pow((2048.0 / 360.0 * (150.0 / 7.0)), -1) * motor.getSensorPosition(); - } - - /** - * Simple encoder logging method - */ - public void logs() { - /*log("Front Right Encoder: " + getFrontRight() + " Front Left Encoder: " + getFrontLeft() - + " Back Right Encoder: " + getBackRight() + " Back Left Encoder: " - + getBackLeft());*/ - } - - /** - * This method is used to set the front right encoder to the true position - */ - public void setFrontRightEncoders() { - //log("Steer FR Before: " + m_SteerFrontRight.getSensorPosition()); - m_SteerFrontRight.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontRight.getAbsolutePosition())); - //log("Steer FR After: " + m_SteerFrontRight.getSensorPosition()); - - } - - /** - * This method is used to set the front left encoder to the true position - */ - public void setFrontLeftEncoders() { - m_SteerFrontLeft.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontLeft.getAbsolutePosition())); - - } - - /** - * This method is used to set the back right encoder to the true position - */ - public void setBackRightEncoders() { - m_SteerBackRight.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackRight.getAbsolutePosition())); - } - - /** - * This method is used to set the back left encoder to the true position - */ - public void setBackLeftEncoders() { - m_SteerBackLeft.setSensorPosition((int) Math - .round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackLeft.getAbsolutePosition())); - } - - // To control each steering individually with a PID - - /** - * This method is used to set the front right steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the front right wheel to - */ - public void setFrontRightAngle(double angle) { - // log("Angle: " + getFrontRight() + " || Motor angle: " + 360.0/ 2048.0 * - // m_SteerFrontRight.getSensorPosition()); - m_SteerFrontRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the front left steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the front left wheel to - */ - public void setFrontLeftAngle(double angle) { - // log("Angle: " + getFrontLeft() + " || Motor angle: " + Math.pow((2048.0/360.0 * - // (150.0/7.0)),-1) * m_SteerFrontLeft.getSensorPosition()); - m_SteerFrontLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the back right steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the back right wheel to - */ - public void setBackRightAngle(double angle) { - // log("Angle: " + getBackRight() + " || Motor angle: " + - // m_SteerBackRight.getSensorPosition()); - m_SteerBackRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * This method is used to set the back left steering motor to a certain angle. This uses a PID - * controller. - * - * @param angle The angle to set the back left wheel to - */ - public void setBackLeftAngle(double angle) { - // log("Angle: " + getBackLeft() + " || Motor angle: " + - // m_SteerBackLeft.getSensorPosition()); - m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle); - } - - /** - * Method to configure PID values. The values were pre-tuned and are not expected to change. - */ - public void configPID() { - // PID for turning the various steering motors. Here is a good link to a tuning website: - // https://www.robotsforroboticists.com/pid-control/ - m_SteerFrontRight.setP(0.2); - m_SteerFrontRight.setI(0); - m_SteerFrontRight.setD(0.1); - m_SteerFrontRight.setFF(0); - - m_SteerFrontLeft.setP(0.2); - m_SteerFrontLeft.setI(0); - m_SteerFrontLeft.setD(0.1); - m_SteerFrontLeft.setFF(0); - - m_SteerBackRight.setP(0.2); - m_SteerBackRight.setI(0); - m_SteerBackRight.setD(0.1); - m_SteerBackRight.setFF(0); - - m_SteerBackLeft.setP(0.2); - m_SteerBackLeft.setI(0); - m_SteerBackLeft.setD(0.1); - // m_SteerBackLeft.setFF(0); - - // pid values from sds for Flacons 500: P = 0.2 I = 0.0 D = 0.1 FF = 0.0 - - // Code to invert sensors if needed. Recommended to do this in phoenix tuner. - // m_SteerFrontRight.setSensorInverted(false); - // m_SteerFrontLeft.setSensorInverted(false); - // m_SteerBackRight.setSensorInverted(false); - // m_SteerBackLeft.setSensorInverted(false); - } - - // Methods to get the encoder values, the encoders are in degrees from -180 to 180. To change - // that, we need to change the syntax and use getPosition() - public double getFrontRight() { - return e_FrontRight.getAbsolutePosition(); - } - - public double getFrontLeft() { - return e_FrontLeft.getAbsolutePosition(); - } - - public double getBackRight() { - return e_BackRight.getAbsolutePosition(); - } - - public double getBackLeft() { - return e_BackLeft.getAbsolutePosition(); - } - - public PointDir getCurrentPosition() { - return currentPosition; - } - - public void setCross() { - checkContextOwnership(); - setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft))); - setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft))); - setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight))); - setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight))); - } - - public void setCurrentPosition(Point P) { - swerveOdometry.setCurrentPosition(P); - } - - public void resetCurrentPosition() { - swerveOdometry.setCurrentPosition(new Point(0, 0)); - } - - /** - * This method is used to reset the drive encoders. - */ - public void resetDriveEncoders() { - m_DriveBackLeft.setSensorPosition(0); - m_DriveBackRight.setSensorPosition(0); - m_DriveFrontLeft.setSensorPosition(0); - m_DriveFrontRight.setSensorPosition(0); - } - - // Odometry - @Override - public void run() { - currentPosition = swerveOdometry.run(); - log(currentPosition.toString()); - //SmartDashboard.putNumber("Front Right Motor Encoder", m_SteerFrontRight.getSensorPosition()); - //SmartDashboard.putNumber("Front Left Motor Encoder", m_SteerFrontLeft.getSensorPosition()); - //SmartDashboard.putNumber("Back Right Motor Encoder", m_SteerBackRight.getSensorPosition()); - SmartDashboard.putNumber("Back Left Motor Encoder", m_SteerBackLeft.getSensorPosition()); - } -} - -// AS diff --git a/src/main/java/com/team766/robot/mechanisms/ReadThisForArms/ReadThis.md b/src/main/java/com/team766/robot/mechanisms/ReadThisForArms/ReadThis.md deleted file mode 100644 index c7f6a879..00000000 --- a/src/main/java/com/team766/robot/mechanisms/ReadThisForArms/ReadThis.md +++ /dev/null @@ -1,11 +0,0 @@ -

Overview and features of Swingy_Arm

-

PID

-

SmartMotion

- The PID for both of the arms is REV Robotics SmartMotion PID. This runs on the rev spark max motorcontroller, which saves memory on the roborio. To tune SmartMotion PID, start tuning kF, then kP, and finally kD. Don't tune kI. - The values must be changed in code, not in the config file (yet, maybe a summer project), by SparkMaxPIDController.setFF(),.setP(), .setD(), you get the idea. -

Switching States

- When we use the PID , we added a buffer of 0.004 absolute encoder units. (oh and i frogot to mention, the PID works on the absolute encoders). - This works in a simple if else loop, and then another loop inside of the if else which only runs if the if is statisfied. The first if loop asks if lastPosition is different. If the new value is different than the lastPosition, then the inside if else will run. Otherwise, the code will apply a steady stream of power to keep the arm where it is. - The next loop asks if the arm position has hit the correct height, with the buffer. If not, it will run the PID. If it has, it will change the feedback device of the motorcontroller to the onboard encoder and then will update lastPosition. This is the same for both arms. -

So yeah, that's about it. Remember to declare your PID variables in the top and lastPosition. Also add your deadzone. Its about 6 variables per arm, and then the deadzone, so 7. Happy coding!

- diff --git a/src/main/java/com/team766/robot/mechanisms/Storage.java b/src/main/java/com/team766/robot/mechanisms/Storage.java deleted file mode 100644 index 01c1a69c..00000000 --- a/src/main/java/com/team766/robot/mechanisms/Storage.java +++ /dev/null @@ -1,26 +0,0 @@ -package com.team766.robot.mechanisms; - -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.hal.RobotProvider; - -public class Storage extends Mechanism { - - private MotorController belt; - - public Storage() { - belt = RobotProvider.instance.getMotor("belt"); - } - - public void beltIn() { - belt.set(0.5); - } - - public void beltOut() { - belt.set(-1.0); - } - - public void beltIdle() { - belt.set(0.0); - } -} diff --git a/src/main/java/com/team766/robot/mechanisms/Wrist.java b/src/main/java/com/team766/robot/mechanisms/Wrist.java new file mode 100644 index 00000000..971c1bea --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Wrist.java @@ -0,0 +1,124 @@ +package com.team766.robot.mechanisms; + +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.Severity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import static com.team766.robot.constants.ConfigConstants.*; + +/** + * Basic wrist mechanism. Used in conjunction with the {@link Intake} and {@link Elevator}. + * Can be moved up and down as part of teleop or autonomous control to move the {@link Intake} + * (attached to the end of the Wrist) closer to a game piece or game element (eg node in the + * field, human player station), at which point the {@link Intake} can grab or release the game + * piece as appropriate. + */ +public class Wrist extends Mechanism { + + /** + * Pre-set positions for the wrist. + */ + public enum Position { + + // TODO: adjust these values. + + /** Wrist is fully up. */ + UP(0.0), + /** Wrist is level with ground. */ + LEVEL(90.0), + /** Wrist is fully down. */ + DOWN(180.0); + + private final double angle; + + Position(double angle) { + this.angle = angle; + } + + private double getAngle() { + return angle; + } + } + + private final CANSparkMax motor; + private final SparkMaxPIDController pidController; + private final ValueProvider pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); + + /** + * Contructs a new Wrist. + */ + public Wrist() { + MotorController halMotor = RobotProvider.instance.getMotor(WRIST_MOTOR); + if (!(halMotor instanceof CANSparkMax)) { + log(Severity.ERROR, "Motor is not a CANSparkMax!"); + throw new IllegalStateException("Motor is not a CANSparkMax!"); + } + motor = (CANSparkMax) halMotor; + + // stash the PIDController for convenience. will update the PID values to the latest from the config + // file each time we use the motor. + pidController = motor.getPIDController(); + pidController.setFeedbackDevice(motor.getEncoder()); + + // grab config values for PID. + pGain = ConfigFileReader.getInstance().getDouble(WRIST_PGAIN); + iGain = ConfigFileReader.getInstance().getDouble(WRIST_IGAIN); + dGain = ConfigFileReader.getInstance().getDouble(WRIST_DGAIN); + ffGain = ConfigFileReader.getInstance().getDouble(WRIST_FFGAIN); + } + + /** + * Returns the current angle of the wrist. + */ + public double getAngle() { + return EncoderUtils.wristEUTodegrees(motor.getEncoder().getPosition()); + } + + /** + * Rotates the wrist to a pre-set {@link Position}. + */ + public void rotate(Position position) { + rotate(position.getAngle()); + } + + /** + * Starts rotating the wrist to the specified angle. + * NOTE: this method returns immediately. Check the current wrist position of the wrist + * with {@link #getAngle()}. + */ + public void rotate(double angle) { + checkContextOwnership(); + + // set the PID controller values with whatever the latest is in the config + pidController.setP(pGain.get()); + pidController.setI(iGain.get()); + pidController.setD(dGain.get()); + pidController.setFF(ffGain.get()); + + pidController.setOutputRange(-1, 1); + + // convert the desired target degrees to encoder units + double encoderUnits = EncoderUtils.wristDegreesToEU(angle); + + // set the reference point for the wrist + pidController.setReference(encoderUnits, ControlType.kPosition); + } + + @Override + public void run() { + if (rateLimiter.next()) { + SmartDashboard.putNumber("WristAngle", getAngle()); + } + } +} diff --git a/src/main/java/com/team766/robot/procedures/AutonLED.java b/src/main/java/com/team766/robot/procedures/AutonLED.java new file mode 100644 index 00000000..f3c699db --- /dev/null +++ b/src/main/java/com/team766/robot/procedures/AutonLED.java @@ -0,0 +1,14 @@ +package com.team766.robot.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Robot; + +public class AutonLED extends Procedure { + public void run (Context context) { + context.takeOwnership(Robot.lights); + Robot.lights.clearAnimation(); + Robot.lights.auton(); + } + +} diff --git a/src/main/java/com/team766/robot/procedures/GyroBalance.java b/src/main/java/com/team766/robot/procedures/GyroBalance.java index 56b76278..d3d51324 100644 --- a/src/main/java/com/team766/robot/procedures/GyroBalance.java +++ b/src/main/java/com/team766/robot/procedures/GyroBalance.java @@ -106,7 +106,7 @@ public void run(Context context) { context.waitForSeconds(CORRECTION_DELAY); // Locks wheels once balanced - context.startAsync(new setCross()); + context.startAsync(new SetCross()); context.releaseOwnership(Robot.drive); context.releaseOwnership(Robot.gyro); diff --git a/src/main/java/com/team766/robot/procedures/IntakeIn.java b/src/main/java/com/team766/robot/procedures/IntakeGrab.java similarity index 51% rename from src/main/java/com/team766/robot/procedures/IntakeIn.java rename to src/main/java/com/team766/robot/procedures/IntakeGrab.java index b9a18c1b..2089d1e0 100644 --- a/src/main/java/com/team766/robot/procedures/IntakeIn.java +++ b/src/main/java/com/team766/robot/procedures/IntakeGrab.java @@ -4,11 +4,11 @@ import com.team766.framework.Procedure; import com.team766.robot.Robot; -public class IntakeIn extends Procedure{ +public class IntakeGrab extends Procedure{ public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.startIntake(); - Robot.storage.beltIn(); + //context.takeOwnership(Robot.intake); + //Robot.intake.in(); + context.waitForSeconds(2); + //Robot.intake.stop(); } } diff --git a/src/main/java/com/team766/robot/procedures/IntakeOut.java b/src/main/java/com/team766/robot/procedures/IntakeRelease.java similarity index 51% rename from src/main/java/com/team766/robot/procedures/IntakeOut.java rename to src/main/java/com/team766/robot/procedures/IntakeRelease.java index 3a891e7f..a97e5c4a 100644 --- a/src/main/java/com/team766/robot/procedures/IntakeOut.java +++ b/src/main/java/com/team766/robot/procedures/IntakeRelease.java @@ -4,11 +4,11 @@ import com.team766.framework.Procedure; import com.team766.robot.Robot; -public class IntakeOut extends Procedure{ +public class IntakeRelease extends Procedure{ public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.reverseIntake(); - Robot.storage.beltOut(); + //context.takeOwnership(Robot.intake); + //Robot.intake.out(); + //context.waitForSeconds(2); + //Robot.intake.stop(); } } \ 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 index ca22867b..d8ae680c 100644 --- a/src/main/java/com/team766/robot/procedures/IntakeStop.java +++ b/src/main/java/com/team766/robot/procedures/IntakeStop.java @@ -6,9 +6,7 @@ public class IntakeStop extends Procedure{ public void run(Context context){ - context.takeOwnership(Robot.intake); - context.takeOwnership(Robot.storage); - Robot.intake.stopIntake(); - Robot.storage.beltIdle(); + //context.takeOwnership(Robot.intake); + //Robot.intake.stop(); } } diff --git a/src/main/java/com/team766/robot/procedures/OPECHelper.java b/src/main/java/com/team766/robot/procedures/OPECHelper.java index b352402e..e4417958 100644 --- a/src/main/java/com/team766/robot/procedures/OPECHelper.java +++ b/src/main/java/com/team766/robot/procedures/OPECHelper.java @@ -4,9 +4,6 @@ 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 { @@ -14,11 +11,11 @@ public class OPECHelper extends Procedure { public void run(Context context) { context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); + // context.takeOwnership(Robot.intake); double startY = Robot.drive.getCurrentPosition().getY(); // robot gyro is offset 90º from how we want, so we reset it to 90º to account for this - Robot.gyro.setGyro(90); - new ReverseIntake().run(context); + Robot.gyro.resetGyro(); + // new IntakeRelease().run(context); Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0); context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST); Robot.drive.stopDrive(); diff --git a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceBalance.java index 4cf1399b..c005c1c7 100644 --- a/src/main/java/com/team766/robot/procedures/OnePieceBalance.java +++ b/src/main/java/com/team766/robot/procedures/OnePieceBalance.java @@ -10,9 +10,9 @@ public class OnePieceBalance extends Procedure { public void run(Context context) { context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); + //context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.setGyro(90); + Robot.gyro.resetGyro(); switch (DriverStation.getAlliance()) { case Blue: Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); @@ -26,7 +26,7 @@ public void run(Context context) { return; } - new ReverseIntake().run(context); + // new IntakeRelease().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 index c29cc9d5..d86e604d 100644 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java +++ b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunity.java @@ -10,9 +10,9 @@ public class OnePieceExitCommunity extends Procedure { public void run (Context context) { context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); + //context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.setGyro(90); + Robot.gyro.resetGyro(); switch (DriverStation.getAlliance()) { case Blue: Robot.drive.setCurrentPosition(new PointDir(2, 0.75)); diff --git a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java index 7fa5da14..b4473515 100644 --- a/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java +++ b/src/main/java/com/team766/robot/procedures/OnePieceExitCommunityBalance.java @@ -10,9 +10,9 @@ public class OnePieceExitCommunityBalance extends Procedure { public void run (Context context) { context.takeOwnership(Robot.drive); - context.takeOwnership(Robot.intake); + //context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); - Robot.gyro.setGyro(90); + Robot.gyro.resetGyro(); switch (DriverStation.getAlliance()) { case Blue: Robot.drive.setCurrentPosition(new PointDir(2.7, 2)); 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 index 1d59cb35..28cec807 100644 --- a/src/main/java/com/team766/robot/procedures/setCross.java +++ b/src/main/java/com/team766/robot/procedures/setCross.java @@ -4,7 +4,7 @@ import com.team766.framework.Procedure; import com.team766.robot.Robot; -public class setCross extends Procedure { +public class SetCross extends Procedure { public void run(Context context) { context.takeOwnership(Robot.drive);