diff --git a/src/main/java/frc/robot/MechanismFlywheel2d.java b/src/main/java/frc/robot/MechanismFlywheel2d.java new file mode 100644 index 0000000..c8c66fd --- /dev/null +++ b/src/main/java/frc/robot/MechanismFlywheel2d.java @@ -0,0 +1,78 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d; + +public class MechanismFlywheel2d extends MechanismObject2d { + + StringPublisher dummyTypePub; + // Only line seems to be supported by glass/advantageScope, so use that + private static String type = "line"; + + private double radius; + private double numSpokes; + + private double currentAngle = 0; + private long lastUpdate = System.currentTimeMillis(); + private double RPM = 0; + + private MechanismLigament2d[] spokes; + + public MechanismFlywheel2d(String name, double radius, int numSpokes) { + super(name); + + this.radius = radius; + this.numSpokes = numSpokes; + + spokes = new MechanismLigament2d[numSpokes]; + for(int i = 0; i < numSpokes; i++){ + MechanismLigament2d spoke = new MechanismLigament2d("Spoke " + i, radius, 0); + append(spoke); + spokes[i] = spoke; + } + update(); + } + + public synchronized void setRPM(double RPM) { + this.RPM = RPM; + } + + public synchronized double getAccumulatedAngle() { + return currentAngle; + } + + public void update(){ + long time = System.currentTimeMillis(); + currentAngle += RPM * 360 * (time - lastUpdate) / 1000 / 60; + lastUpdate = time; + + + double dTheta = 360 / numSpokes; + for(int i = 0; i < numSpokes; i++) { + spokes[i].setAngle(i * dTheta + currentAngle); + } + } + + @Override + protected void updateEntries(NetworkTable table) { + if(dummyTypePub != null){ + dummyTypePub.close(); + } + dummyTypePub = table.getStringTopic(".type").publishEx(StringTopic.kTypeString, "{\"SmartDashboard\":\"" + type + "\"}"); + dummyTypePub.set(type); + } + + @Override + public void close(){ + super.close(); + if(dummyTypePub != null){ + dummyTypePub.close(); + } + for (MechanismLigament2d spoke : spokes) { + spoke.close(); + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..79c9890 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -14,9 +17,11 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ +@Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { private Command m_autonomousCommand; + @Logged(name="Container") private RobotContainer m_robotContainer; /** @@ -25,6 +30,8 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + Epilogue.bind(this); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f10e4cb..e8c08e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -12,19 +13,21 @@ /* IMPORT SUBSYSTEMS */ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Camera; - +import frc.robot.subsystems.Crusher; +import frc.robot.commands.CrushDefaultCommand; /* IMPORT COMMANDS */ import frc.robot.commands.DriveCommand; import frc.robot.commands.auto.DriveTowardsRing; +@Logged public class RobotContainer { /** CREATE SUBSYSTEMS */ private final Drivetrain drivetrain = new Drivetrain(RobotMap.leftDrive1, RobotMap.rightDrive1); private final Camera camera = new Camera(); - + public final Crusher crusher = new Crusher(RobotMap.crusherMotor); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -33,6 +36,7 @@ public RobotContainer() { configureBindings(); drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.leftDriveSupplier, OI.rightDriveSupplier)); + crusher.setDefaultCommand(new CrushDefaultCommand(crusher)); } /** diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index faeb8bf..fd186f0 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -1,10 +1,9 @@ package frc.robot; import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; - -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class RobotMap { @@ -13,11 +12,15 @@ public class RobotMap { private static final int RIGHT_DRIVE_1 = 1; private static final int RIGHT_DRIVE_2 = 2; + private static final int CRUSHER_MOTOR = 5; + public static final BaseMotorController leftDrive1; public static final BaseMotorController leftDrive2; public static final BaseMotorController rightDrive1; public static final BaseMotorController rightDrive2; + public static final WPI_TalonSRX crusherMotor; + static { leftDrive1 = new VictorSPX(LEFT_DRIVE_1); leftDrive2 = new VictorSPX(LEFT_DRIVE_2); @@ -26,5 +29,7 @@ public class RobotMap { rightDrive1 = new VictorSPX(RIGHT_DRIVE_1); rightDrive2 = new VictorSPX(RIGHT_DRIVE_2); rightDrive2.follow(rightDrive1); + + crusherMotor = new WPI_TalonSRX(CRUSHER_MOTOR); } } diff --git a/src/main/java/frc/robot/commands/CrushDefaultCommand.java b/src/main/java/frc/robot/commands/CrushDefaultCommand.java new file mode 100644 index 0000000..dd859d5 --- /dev/null +++ b/src/main/java/frc/robot/commands/CrushDefaultCommand.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Crusher; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class CrushDefaultCommand extends Command { + + Crusher crusher; + + /** Creates a new CrushDefaultCommand. */ + public CrushDefaultCommand(Crusher crusher) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(crusher); + + this.crusher = crusher; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + crusher.setOutput(0.5); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Crusher.java b/src/main/java/frc/robot/subsystems/Crusher.java new file mode 100644 index 0000000..0893e16 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Crusher.java @@ -0,0 +1,114 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.MechanismFlywheel2d; +import frc.robot.Robot; + +@Logged(strategy = Strategy.OPT_IN) +public class Crusher extends SubsystemBase { + + // Use WPI_TalonSRX for better simulation support + WPI_TalonSRX talon; + + // 12.75:1 Gearbox, 4:1 belt reduction to flywheel + private static final double GEAR_REDUCTION = 12.75; + // 11cm ID to 15cm OD radius ring, 20lbs + private static final double MOMENT_OF_INERTIA = 0.5 * Units.lbsToKilograms(20) * (0.11 * 0.11 + 0.15 * 0.15); + + LinearSystem sys = LinearSystemId.createFlywheelSystem(DCMotor.getCIM(1), MOMENT_OF_INERTIA, GEAR_REDUCTION); + FlywheelSim flywheel = new FlywheelSim(sys, DCMotor.getCIM(1)); + TalonSRXSimCollection simTalon; + + Mechanism2d display = new Mechanism2d(3, 3); + MechanismFlywheel2d flywheelDisplay; + MechanismLigament2d crankshaft; + MechanismLigament2d crankArm; + MechanismLigament2d crushArm; + + private double crankshaftRatio = 1 / 4d; + private double crankshaftLength = 0.15; + private double crankArmlength = 0.25; + private double crushArmLength = 0.25; + + public double crushPos; + + /** Creates a new Crusher. */ + public Crusher(WPI_TalonSRX talon) { + this.talon = talon; + if (Robot.isSimulation()){ + simTalon = talon.getSimCollection(); + } + + MechanismRoot2d root = display.getRoot("Flywheel Root", 1.5, 1.5); + flywheelDisplay = new MechanismFlywheel2d("Flywheel", 0.15, 6); + root.append(flywheelDisplay); + + MechanismRoot2d crankshaftRoot = display.getRoot("Crankshaft Root", 1.5, 1); + crankshaft = new MechanismLigament2d("Crankshaft", crankshaftLength, 0); + crankArm = new MechanismLigament2d("Crank Arm", crankArmlength, 0); + crushArm = new MechanismLigament2d("Crush Arm", crushArmLength, 0); + crankshaftRoot.append(crankshaft); + crankshaft.append(crankArm); + crankArm.append(crushArm); + + SmartDashboard.putData("Flywheel", display); + } + + public void setOutput(double output){ + talon.set(ControlMode.PercentOutput, output); + } + + @Logged(name="Curent (A)") + public double getCurrent(){ + return talon.getStatorCurrent(); + } + + @Logged(name="Motor Volts (V)") + public double getOutput(){ + return talon.getMotorOutputVoltage(); + } + + @Logged(name="RPM [SIMULATION]") + public double simRPM(){ + return flywheel.getAngularVelocityRPM(); + } + + @Override + public void simulationPeriodic() { + simTalon.setBusVoltage(12); + flywheel.setInputVoltage(simTalon.getMotorOutputLeadVoltage()); + flywheel.update(0.02); + simTalon.setStatorCurrent(flywheel.getCurrentDrawAmps()); + + flywheelDisplay.update(); + flywheelDisplay.setRPM(flywheel.getAngularVelocityRPM()); + + double crankshaftAngle = (flywheelDisplay.getAccumulatedAngle() * crankshaftRatio) % 360; + crankshaft.setAngle(crankshaftAngle); + double armAngle = Math.toDegrees(-Math.asin(crankshaftLength / crankArmlength * Math.sin(Math.toRadians(crankshaftAngle)))) - crankshaftAngle; + crankArm.setAngle(armAngle); + double crushAngle = -(crankshaftAngle + armAngle); + crushArm.setAngle(crushAngle); + + // Find the overall distance from rotation point to crusher end + crushPos = crankshaftLength * Math.cos(Math.toRadians(crankshaftAngle)) + crankArmlength * Math.cos(Math.toRadians(crankshaftAngle + armAngle)) + crushArmLength; + SmartDashboard.putNumber("Crush Pos", crushPos); + } +}