Skip to content

Commit

Permalink
Fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@BenG49
BenG49 committed Jan 10, 2024
1 parent 5f694af commit 93e42d9
Show file tree
Hide file tree
Showing 5 changed files with 59 additions and 247 deletions.
157 changes: 1 addition & 156 deletions src/main/java/com/stuypulse/robot/constants/Field.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,6 @@
import com.stuypulse.stuylib.network.SmartNumber;

import com.stuypulse.robot.RobotContainer;
// import com.stuypulse.robot.subsystems.Manager.ScoreSide;
// import com.stuypulse.robot.util.AllianceUtil;
import com.stuypulse.robot.util.Fiducial;

import edu.wpi.first.math.geometry.Pose2d;
Expand Down Expand Up @@ -59,160 +57,7 @@ public static double[] getTagLayout(Fiducial[] fiducials) {
return layout;
}

//XXX: Commented out from Jim's code
// SmartNumber CHARGING_STATION_CENTER = new SmartNumber("Field/Charging Station Center", 172.631 - 14);
// double PEG_TO_CHARGING_STATION_EDGE = Units.inchesToMeters(60.69);

// double GRID_DEPTH = Units.inchesToMeters(54.25);

// // intake offset from center to the right
// double INTAKE_OFFSET_RIGHT = Units.inchesToMeters(1.625);

// Pose2d BLUE_APRIL_TAGS[] = {
// // 1-4
// new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), Rotation2d.fromDegrees(180)),
// new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), Rotation2d.fromDegrees(180)),
// new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), Rotation2d.fromDegrees(180)),
// new Pose2d(WIDTH - Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), Rotation2d.fromDegrees(180)),

// // 5-8
// new Pose2d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), new Rotation2d(0)),
// new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), new Rotation2d(0)),
// new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), new Rotation2d(0)),
// new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), new Rotation2d(0)),
// };

public static boolean isValidAprilTagId(int id) {
return id >= 1 && id <= 8;
return id >= 1 && id <= 16;
}

// public static Pose2d getAprilTagFromId(int id) {
// if (RobotContainer.getCachedAlliance() == Alliance.Blue) {
// return BLUE_APRIL_TAGS[id - 1];
// } else {
// return AllianceUtil.getMirroredPose(BLUE_APRIL_TAGS[8 - id]);
// }
// }

public interface Pegs {
// LAB VALUES
double RED_Y[] = {
7.494778 - 0.075,
6.935978 - 0.075,
6.377178 - 0.075,
5.843778 - 0.075,
5.259578 - 0.075,
4.700778 - 0.075,
4.141978 - 0.075,
3.583178 - 0.075,
3.024378 - 0.075
};

double BLUE_Y[] = {
4.9784 - 0.06,
4.4196 - 0.06,
3.8608 - 0.06,
3.3020 - 0.06,
2.7432 - 0.06,
2.1844 - 0.06,
1.6256 - 0.06,
1.0668 - 0.06,
0.5080 - 0.06
};
}

//XXX: Commented out from Jim's code
// static class RedBlueNumber extends Number { private double red, blue; public RedBlueNumber(double red, double blue) { this.red = red; this.blue = blue; } public double doubleValue() { if (RobotContainer.getCachedAlliance() == Alliance.Red) return red; return blue; } public int intValue() { return (int) doubleValue(); } public float floatValue() { return (float) doubleValue(); } public long longValue() { return (long) doubleValue(); } }

// public interface ScoreXPoses {
// public interface High {
// Number CUBE_BACK = 1.846;
// Number CUBE_FRONT = 1.825;
// Number CONE_TIP_IN = 1.75;
// Number CONE_TIP_OUT = 1.772 - Units.inchesToMeters(2);
// }

// public interface Mid {
// Number CUBE_BACK = 1.881;
// Number CUBE_FRONT = 2.106;
// Number CONE_TIP_IN = 2.21;
// Number CONE_TIP_OUT = 2.18;
// }

// public interface Low {
// Number BACK = 1.9;
// Number FRONT = 1.825;
// }
// }

// // red left to right
// public interface ScoreYPoses {
// public static double[] getYPoseArray(Alliance alliance, ScoreSide side) {
// if (side == ScoreSide.FRONT)
// return alliance == Alliance.Red ? Front.RED_Y_POSES : Front.BLUE_Y_POSES;
// else
// return alliance == Alliance.Red ? Back.RED_Y_POSES : Back.BLUE_Y_POSES;
// }

// public static double middleToBack(double midYPose) {
// return midYPose + Field.INTAKE_OFFSET_RIGHT;
// }

// public interface Back {
// double RED_Y_POSES[] = {
// middleToBack(Pegs.RED_Y[0]),
// middleToBack(Pegs.RED_Y[1]),
// middleToBack(Pegs.RED_Y[2]),
// middleToBack(Pegs.RED_Y[3]),
// middleToBack(Pegs.RED_Y[4]),
// middleToBack(Pegs.RED_Y[5]),
// middleToBack(Pegs.RED_Y[6]),
// middleToBack(Pegs.RED_Y[7]),
// middleToBack(Pegs.RED_Y[8])
// };

// double BLUE_Y_POSES[] = {
// middleToBack(Pegs.BLUE_Y[0]),
// middleToBack(Pegs.BLUE_Y[1]),
// middleToBack(Pegs.BLUE_Y[2]),
// middleToBack(Pegs.BLUE_Y[3]),
// middleToBack(Pegs.BLUE_Y[4]),
// middleToBack(Pegs.BLUE_Y[5]),
// middleToBack(Pegs.BLUE_Y[6]),
// middleToBack(Pegs.BLUE_Y[7]),
// middleToBack(Pegs.BLUE_Y[8])
// };
// }

// public interface Front {
// public static double middleToFront(double midYPose) {
// return midYPose - Field.INTAKE_OFFSET_RIGHT;
// }

// double RED_Y_POSES[] = {
// middleToFront(Pegs.RED_Y[0]),
// middleToFront(Pegs.RED_Y[1]),
// middleToFront(Pegs.RED_Y[2]),
// middleToFront(Pegs.RED_Y[3]),
// middleToFront(Pegs.RED_Y[4]),
// middleToFront(Pegs.RED_Y[5]),
// middleToFront(Pegs.RED_Y[6]),
// middleToFront(Pegs.RED_Y[7]),
// middleToFront(Pegs.RED_Y[8])
// };

// double BLUE_Y_POSES[] = {
// middleToFront(Pegs.BLUE_Y[0]),
// middleToFront(Pegs.BLUE_Y[1]),
// middleToFront(Pegs.BLUE_Y[2]),
// middleToFront(Pegs.BLUE_Y[3]),
// middleToFront(Pegs.BLUE_Y[4]),
// middleToFront(Pegs.BLUE_Y[5]),
// middleToFront(Pegs.BLUE_Y[6]),
// middleToFront(Pegs.BLUE_Y[7]),
// middleToFront(Pegs.BLUE_Y[8])
// };
// }
// }

}
6 changes: 6 additions & 0 deletions src/main/java/com/stuypulse/robot/constants/Motors.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkMax;
import static com.revrobotics.CANSparkMax.IdleMode;

Expand All @@ -27,6 +28,11 @@ public interface Drivetrain {
CANSparkMaxConfig RIGHT = new CANSparkMaxConfig(true, IdleMode.kBrake, 60);
}

public interface Launcher {
CANSparkMaxConfig LAUNCHER = new CANSparkMaxConfig(false, IdleMode.kCoast, 80);
CANSparkMaxConfig FEEDER = new CANSparkMaxConfig(false, IdleMode.kCoast, 80);
}

/** Classes to store all of the values a motor needs */

public static class TalonSRXConfig {
Expand Down
7 changes: 1 addition & 6 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,16 +18,11 @@
*/
public interface Settings {
public interface Drivetrain {
int CURRENT_LIMIT = 60;

//TODO: ask rain for true track width currently using cad with +-0.1
double TRACK_WIDTH = Units.inchesToMeters(23.83);
double TRACK_WIDTH = Units.inchesToMeters(23.83);
}

public interface Launcher {
int LAUNCHER_CURRENT_LIMIT = 80;
int FEEDER_CURRENT_LIMIT = 80;

// Speeds for wheels when intaking and launching. Intake speeds are negative to run the wheels
// in reverse
double LAUNCH_LAUNCHER_SPEED = 1;
Expand Down
112 changes: 36 additions & 76 deletions src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java
View file
Original file line number Diff line number Diff line change
@@ -1,82 +1,58 @@
package com.stuypulse.robot.subsystems.drivetrain;

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.stuypulse.robot.constants.Motors;
import com.stuypulse.robot.constants.Ports;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.stuylib.math.Angle;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Drivetrain extends SubsystemBase{
private static Drivetrain instance = new Drivetrain();
public class Drivetrain extends SubsystemBase {

private static final Drivetrain instance;

static {
instance = new Drivetrain();
}

public static Drivetrain getInstance() {
return instance;
}

private DifferentialDrive drivetrain;

CANSparkMax leftFront;
CANSparkMax leftBack;
CANSparkMax rightFront;
CANSparkMax rightBack;
private final DifferentialDrive drivetrain;

RelativeEncoder leftEncoder;
RelativeEncoder rightEncoder;
private final CANSparkMax leftFront;
private final CANSparkMax leftBack;
private final CANSparkMax rightFront;
private final CANSparkMax rightBack;

public Drivetrain() {
leftFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
leftBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
rightFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
rightBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed);
leftBack = new CANSparkMax(Ports.Drivetrain.LEFTREAR, MotorType.kBrushed);
rightFront = new CANSparkMax(Ports.Drivetrain.RIGHTFRONT, MotorType.kBrushed);
rightBack = new CANSparkMax(Ports.Drivetrain.RIGHTREAR, MotorType.kBrushed);

Motors.Drivetrain.LEFT.configure(leftFront);
Motors.Drivetrain.LEFT.configure(leftBack);

Motors.Drivetrain.RIGHT.configure(rightFront);
Motors.Drivetrain.RIGHT.configure(rightBack);

leftBack.follow(leftFront);
rightBack.follow(rightFront);

leftFront.setInverted(true);
rightFront.setInverted(false);

leftFront.setSmartCurrentLimit(Settings.Drivetrain.CURRENT_LIMIT);
leftBack.setSmartCurrentLimit(Settings.Drivetrain.CURRENT_LIMIT);
rightFront.setSmartCurrentLimit(Settings.Drivetrain.CURRENT_LIMIT);
leftBack.setSmartCurrentLimit(Settings.Drivetrain.CURRENT_LIMIT);

leftEncoder = leftFront.getEncoder();
rightEncoder = rightFront.getEncoder();
resetEncoders();

drivetrain = new DifferentialDrive(leftFront, rightFront);
}

//********** GETTERS **********
// Distance
public double getLeftDistance() {
return leftEncoder.getPosition();
}

public double getRightDistance() {
return rightEncoder.getPosition();
}

public double getDistance() {
return (getLeftDistance() + getRightDistance()) / 2.0;
//********** GETTERS **********//
public double getLeftSpeed() {
return leftFront.getEncoder().getVelocity();
}

// Velocity
public double getLeftVelocity() {
return leftEncoder.getVelocity();
}

public double getRightVelocity() {
return rightEncoder.getVelocity();
}

public double getVelocity() {
return (getLeftVelocity() + getRightVelocity()) / 2.0;
public double getRightSpeed() {
return rightFront.getEncoder().getVelocity();
}

public double getLeftVoltage() {
Expand All @@ -87,21 +63,15 @@ public double getRightVoltage() {
return rightFront.getAppliedOutput();
}

private double getRawEncoderAngle() {
double distance = getLeftDistance() - getRightDistance();
return Math.toDegrees(distance / Settings.Drivetrain.TRACK_WIDTH);
}

public Angle getAngle() {
return Angle.fromDegrees(getRawEncoderAngle());
public double getLeftDistance() {
return leftFront.getEncoder().getPosition();
}

public void resetEncoders() {
leftEncoder.setPosition(0.0);
rightEncoder.setPosition(0.0);
public double getRightDistance() {
return rightFront.getEncoder().getPosition();
}

//********** Drive Methods **********
//********** Drive Methods **********//
public void tankDrive(double leftSpeed, double rightSpeed) {
drivetrain.tankDrive(leftSpeed, rightSpeed);
}
Expand All @@ -120,19 +90,9 @@ public void stop() {

@Override
public void periodic() {
SmartDashboard.putNumber("Drivetrain/ Left Velocity (m per s)", getLeftVelocity());
SmartDashboard.putNumber("Drivetrain/ Right Velocity (m per s)", getRightVelocity());

SmartDashboard.putNumber("Drivetrain/ Left Distance (m)", getLeftDistance());
SmartDashboard.putNumber("Drivetrain/ Right Distance (m)", getRightDistance());

SmartDashboard.putNumber("Drivetrain/ Angle (deg)", getAngle().toDegrees());


SmartDashboard.putNumber("Drivetrain/ Left Voltage (V)", getLeftVoltage());
SmartDashboard.putNumber("Drivetrain/ Right Voltage (V)", getRightVoltage());

SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed());
SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());
SmartDashboard.putNumber("Drivetrain/Left Voltage", getLeftVoltage());
SmartDashboard.putNumber("Drivetrain/Right Voltage", getRightVoltage());
}


}
Loading

0 comments on commit 93e42d9

Please sign in to comment.