Advay balakrishnan sw06a

src/main/java/frc/robot/Constants.java

@@ -13,4 +13,21 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static class FlywheelConstants {
+        public static final int LEFT_CAN_ID = 10;
+        public static final int RIGHT_CAN_ID = 11;
+
+        public static final int KP = 1;
+        public static final int KD = 0;
+        public static final int KI = 0;
+
+    }
+
+    public static class TurrentConstants {
+        public static final int TURRENT_ID = 12;
+
+        public static final int KP = 1;
+        public static final int KD = 0;
+        public static final int KI = 0;
+    }
 }

src/main/java/frc/robot/flywheel/FlywheelSubsystem.java

@@ -0,0 +1,61 @@
+package frc.robot.flywheel;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import static frc.robot.Constants.FlywheelConstants.*;
+
+/*
+ * A class to control the hardware associated with the flywheeel subsystem
+ */
+// TODO: Finish this class
+public class FlywheelSubsystem extends SubsystemBase {
+    private WPI_TalonFX leftMaster;
+    private WPI_TalonFX rightFollower;
+
+    /*
+     * Initialize hardware
+     * 1 Master WPI_TalonFX (Left)
+     * 1 Follower WPI_TalonFX (Right)
+     * Total: 2 TalonFX motors
+     */
+    public FlywheelSubsystem() {
+        leftMaster = new WPI_TalonFX(LEFT_CAN_ID);
+        rightFollower = new WPI_TalonFX(RIGHT_CAN_ID);
+        rightFollower.follow(leftMaster);
+
+    }
+
+    /*
+     * Stop the flywheel from moving
+     */
+    public void stopFlywheel() {
+        leftMaster.neutralOutput();
+    }
+
+    /*
+     * Set the voltage of the motor 
+     */
+    public void setVoltage(double voltage) {
+        leftMaster.setVoltage(voltage);
+    }
+
+    /*
+     * Get the selected sensor velocity of the master motor and convert to rpm
+     */
+    public double getVelocity() {
+        // conversion factor: 600/2048;
+        return leftMaster.getSelectedSensorVelocity()*600/2048;
+    }
+
+    /*
+     * Get the selected sensor velocity of the following motor
+     */
+    public double getFollowingVelocity() {
+        // conversion factor: 600/2048;
+        return rightFollower.getSelectedSensorVelocity()*600/2048;
+    }
+
+
+}

src/main/java/frc/robot/flywheel/commands/SetFlywheelVelocityFromPID.java

@@ -0,0 +1,42 @@
+package frc.robot.flywheel.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import static frc.robot.Constants.FlywheelConstants.*;
+import frc.robot.flywheel.FlywheelSubsystem;
+
+/*
+ * Set the velocity of the flywheel using PID
+ */
+
+public class SetFlywheelVelocityFromPID extends PIDCommand {
+    DoubleSupplier velocity;
+    FlywheelSubsystem flywheelSubsystem;
+
+    public SetFlywheelVelocityFromPID(FlywheelSubsystem flywheelSubsystem, DoubleSupplier velocity) {
+        super(
+                new PIDController(KP, KI, KD),
+                flywheelSubsystem::getVelocity,
+                velocity,
+                flywheelSubsystem::setVoltage,
+                flywheelSubsystem
+             );
+
+        this.velocity = velocity;
+        this.flywheelSubsystem = flywheelSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        SmartDashboard.putNumber("Flywheel Velocity", flywheelSubsystem.getVelocity());
+    }
+
+    @Override
+    public boolean isFinished(){
+        return getController().atSetpoint();
+    }
+}

src/main/java/frc/robot/flywheel/commands/TrackAndShoot.java

@@ -0,0 +1,29 @@
+package frc.robot.flywheel.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.flywheel.FlywheelSubsystem;
+import frc.robot.hardware.Limelight;
+import frc.robot.turret.TurretSubsystem;
+import frc.robot.turret.commands.SetTurretPosition;
+
+
+
+// TODO: Make this extend a command grouper and finish this class
+public class TrackAndShoot extends ParallelCommandGroup {
+    FlywheelSubsystem flywheelSubsystem;
+    TurretSubsystem turretSubsystem;
+
+    // Run both the SetFlywheelVelocity command and SetTurretPosition commands 
+    public TrackAndShoot(FlywheelSubsystem flywheelSubsystem, TurretSubsystem turretSubsystem) {
+        this.flywheelSubsystem = flywheelSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        addCommands (
+                new SetFlywheelVelocityFromPID(flywheelSubsystem, ()->2000),
+                new SetTurretPosition(turretSubsystem,Limelight::getTx)
+        );
+
+    }
+}

src/main/java/frc/robot/hardware/Limelight.java

@@ -0,0 +1,7 @@
+package frc.robot.hardware;
+
+public class Limelight {
+    public static double getTx() {
+        return 3000;
+    } 
+}

src/main/java/frc/robot/turret/TurretSubsystem.java

@@ -0,0 +1,39 @@
+package frc.robot.turret;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import static frc.robot.Constants.TurrentConstants.*;
+
+/*
+ * A class to control the hardware associated with the turret subsystem
+ */
+
+public class TurretSubsystem extends SubsystemBase {
+    private TalonFX turretMotor;
+
+    /*
+     * Initialize hardware
+     * 1 Master TalonFX 
+     */
+    public TurretSubsystem() {
+        turretMotor = new TalonFX(TURRENT_ID);
+    }
+
+    /*
+     * Set the position of the turrent
+     */
+    public void setPosition(double position) {
+        turretMotor.set(ControlMode.Position, position);
+    }
+
+    /*
+     * Get the current position of the turret
+     */
+    public double getPosition() {
+        return turretMotor.getSelectedSensorPosition();
+    }
+}
+

src/main/java/frc/robot/turret/commands/SetTurretPosition.java

@@ -0,0 +1,39 @@
+package frc.robot.turret.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import static frc.robot.Constants.TurrentConstants.*;
+import frc.robot.turret.TurretSubsystem;
+
+
+public class SetTurretPosition extends PIDCommand {
+    DoubleSupplier position;
+    TurretSubsystem turretSubsystem;
+
+    public SetTurretPosition(TurretSubsystem turretSubsystem, DoubleSupplier position) {
+        super(
+                new PIDController(KP, KI, KD),
+                turretSubsystem::getPosition,
+                position,
+                turretSubsystem::setPosition,
+                turretSubsystem
+             );
+
+        this.position = position;
+        this.turretSubsystem = turretSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        SmartDashboard.putNumber("Turret Position", turretSubsystem.getPosition());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return getController().atSetpoint();
+    }
+}

src/test/java/robot/flywheel/SetFlywheelVelocityFromPIDTest.java

@@ -0,0 +1,48 @@
+package robot.flywheel;
+
+import static org.junit.Assert.*;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.flywheel.FlywheelSubsystem;
+import frc.robot.flywheel.commands.SetFlywheelVelocityFromPID;
+import frc.robot.turret.TurretSubsystem;
+import frc.robot.turret.commands.SetTurretPosition;
+
+import org.junit.*;
+
+public class SetFlywheelVelocityFromPIDTest {
+    public static final double DELTA = 10; // acceptable deviation range
+    FlywheelSubsystem flywheelSubsystem;
+
+    @Before
+    public void setup() {
+        assert HAL.initialize(500, 0); // initialize the HAL, crash if failed
+        CommandScheduler.getInstance().enable(); 
+        DriverStationSim.setEnabled(true);
+
+        flywheelSubsystem = new FlywheelSubsystem();
+    }
+
+    @Test
+    public void setVelocity2000(){
+        SetFlywheelVelocityFromPID setVelocity = new SetFlywheelVelocityFromPID(flywheelSubsystem, () -> 2000);
+        CommandScheduler.getInstance().schedule(setVelocity);
+
+        runScheduler(1);
+        System.out.println(flywheelSubsystem.getVelocity());
+        assertEquals("Sets velocity to 2000 rpm", flywheelSubsystem.getVelocity(), 2000, DELTA);
+    }
+
+     private static void runScheduler(double seconds) {
+        try {
+            for (int i = 0; i < 10; ++i) {
+                com.ctre.phoenix.unmanaged.Unmanaged.feedEnable(100);
+                CommandScheduler.getInstance().run();
+                Thread.sleep((long) (seconds * 100));
+            }
+        } catch (InterruptedException e) {
+            e.printStackTrace();
+        }
+    }
+}

src/test/java/robot/flywheel/TrackAndShootTest.java

@@ -0,0 +1,38 @@
+package robot.flywheel;
+
+import static org.junit.Assert.*;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.flywheel.FlywheelSubsystem;
+import frc.robot.flywheel.commands.SetFlywheelVelocityFromPID;
+import frc.robot.turret.TurretSubsystem;
+import frc.robot.turret.commands.SetTurretPosition;
+
+import org.junit.*;
+
+public class TrackAndShootTest {
+    public static final double DELTA = 10; // acceptable deviation range
+    FlywheelSubsystem flywheelSubsystem;
+
+    @Before
+    public void setup() {
+        assert HAL.initialize(500, 0); // initialize the HAL, crash if failed
+        CommandScheduler.getInstance().enable(); 
+        DriverStationSim.setEnabled(true);
+
+        flywheelSubsystem = new FlywheelSubsystem();
+    }
+
+    private static void runScheduler(double seconds) {
+        try {
+            for (int i = 0; i < 10; ++i) {
+                com.ctre.phoenix.unmanaged.Unmanaged.feedEnable(100);
+                CommandScheduler.getInstance().run();
+                Thread.sleep((long) (seconds * 100));
+            }
+        } catch (InterruptedException e) {
+            e.printStackTrace();
+        }
+    }
+}

src/test/java/robot/turret/SetTurretPositionTest.java

@@ -0,0 +1,49 @@
+package robot.flywheel;
+
+import static org.junit.Assert.*;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.flywheel.FlywheelSubsystem;
+import frc.robot.flywheel.commands.SetFlywheelVelocityFromPID;
+import frc.robot.turret.TurretSubsystem;
+import frc.robot.turret.commands.SetTurretPosition;
+
+import org.junit.*;
+
+public class SetTurretPositionTest {
+    public static final double DELTA = 10; // acceptable deviation range
+    TurretSubsystem turretSubsystem;
+
+    @Before
+    public void setup() {
+        assert HAL.initialize(500, 0); // initialize the HAL, crash if failed
+        CommandScheduler.getInstance().enable(); 
+        DriverStationSim.setEnabled(true);
+
+        turretSubsystem = new TurretSubsystem();
+    }
+
+    @Test
+    public void setTurretPosition(){
+        SetTurretPosition setTurretPosition = new SetTurretPosition(turretSubsystem, () -> 3000000);
+        turretSubsystem.setPosition(3000);
+        CommandScheduler.getInstance().schedule(setTurretPosition);
+
+        runScheduler(5);
+        System.out.println(turretSubsystem.getPosition());
+        assertEquals("Set turret position to 3000", turretSubsystem.getPosition(), 3000, DELTA);
+    }
+
+    private static void runScheduler(double seconds) {
+        try {
+            for (int i = 0; i < seconds * 1000 / 20; ++i) {
+                com.ctre.phoenix.unmanaged.Unmanaged.feedEnable(100);
+                CommandScheduler.getInstance().run();
+                Thread.sleep(20);
+            }
+        } catch (InterruptedException e) {
+            e.printStackTrace();
+        }
+    }
+}