Shooter commands

src/main/java/org/usfirst/frc4904/robot/RobotMap.java

@@ -1,75 +1,190 @@
 package org.usfirst.frc4904.robot;
 
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+import org.usfirst.frc4904.robot.subsystems.Indexer;
+import org.usfirst.frc4904.robot.subsystems.Intake;
+import org.usfirst.frc4904.robot.subsystems.Shooter;
+import org.usfirst.frc4904.standard.custom.PCMPort;
 import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick;
 import org.usfirst.frc4904.standard.custom.controllers.CustomXbox;
-
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonFX;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.ContinuousServoController;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController;
+import org.usfirst.frc4904.standard.custom.sensors.CANEncoder;
+import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder;
+import org.usfirst.frc4904.standard.custom.sensors.CustomCANCoder;
+import org.usfirst.frc4904.standard.custom.sensors.CustomDigitalLimitSwitch;
+import org.usfirst.frc4904.standard.subsystems.SolenoidSubsystem;
+import org.usfirst.frc4904.standard.subsystems.motor.Motor;
+import org.usfirst.frc4904.robot.subsystems.Hood;
+
+import edu.wpi.first.wpilibj.PWMSpeedController;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 
 public class RobotMap {
-    public static class Port {
-        public static class HumanInput {
-            public static final int joystick = 0;
-            public static final int xboxController = 1;
-        }
-
-        public static class CANMotor {
-        }
-
-        public static class PWM {
-        }
-
-        public static class CAN {
-        }
-
-        public static class Pneumatics {
-        }
-
-        public static class Digital {
-        }
+  public static class Port {
+    public static class HumanInput {
+      public static final int joystick = 0;
+      public static final int xboxController = 1;
     }
 
-    public static class Metrics {
-        public static class Chassis {
-            public static final double TICKS_PER_REVOLUTION = -1; // TODO: CHANGE CONSTS
-            public static final double DIAMETER_INCHES = -1;
-            public static final double CIRCUMFERENCE_INCHES = Metrics.Chassis.DIAMETER_INCHES * Math.PI;
-            public static final double TICKS_PER_INCH = Metrics.Chassis.TICKS_PER_REVOLUTION
-                    / Metrics.Chassis.CIRCUMFERENCE_INCHES;
-            public static final double DISTANCE_FRONT_BACK = -1;
-            public static final double DISTANCE_SIDE_SIDE = -1;
-            public static final double INCHES_PER_TICK = Metrics.Chassis.CIRCUMFERENCE_INCHES
-                    / Metrics.Chassis.TICKS_PER_REVOLUTION;
-        }
+    public static class CANMotor { // TODO: CHANGE CONSTS
+      // Intake
+      public static final int INTAKE_ROLLER_MOTOR = -1;
+      public static final int INTAKE_FUNNEL_MOTOR = -1;
+      // Indexer
+      public static final int LIFT_BELT_MOTOR = -1;
+      // Flywheel
+      public static final int FLYWHEEL_MOTOR_A = -1;
+      public static final int FLYWHEEL_MOTOR_B = -1;
+      // Shooter
+      public static final int RUN_UP_BELT_MOTOR = -1;
     }
 
-    public static class PID {
-        public static class Drive {
-        }
+    public static class PWM {
+      public static final int HOOD_MOTOR = -1;
+    }
 
-        public static class Turn {
-        }
+    public static class CAN {
+      public static final int HOOD_ENCODER = -1;
+    }
 
+    public static class Pneumatics {
+      public static final PCMPort INTAKE_SOLENOID = new PCMPort(0, -1, -1);
+      public static final PCMPort FLIPPER_SOLENOID = new PCMPort(0, -1, -1);
+      public static final PCMPort SHOOTER_AIM_SOLENOID = new PCMPort(0, -1, -1);
     }
 
-    public static class Component {
+    public static class Digital {
+      public static final int INDEXER_LIMIT_SWITCH = -1;
+    }
+  }
+
+  public static class Metrics {
+    public static class Chassis {
+      public static final double TICKS_PER_REVOLUTION = -1; // TODO: CHANGE CONSTS
+      public static final double DIAMETER_METERS = -1;
+      public static final double CIRCUMFERENCE_METERS = Metrics.Chassis.DIAMETER_METERS * Math.PI;
+      public static final double TICKS_PER_INCH = Metrics.Chassis.TICKS_PER_REVOLUTION
+          / Metrics.Chassis.CIRCUMFERENCE_METERS;
+      public static final double DISTANCE_FRONT_BACK = -1;
+      public static final double DISTANCE_SIDE_SIDE = -1;
+      public static final double METERS_PER_TICK = Metrics.Chassis.CIRCUMFERENCE_METERS
+          / Metrics.Chassis.TICKS_PER_REVOLUTION;
+    }
+    public static class Flywheel {
+      public static final double ROTATIONS_PER_TICK = 1.0 / 2048.0;
+    }
+  }
+
+  public static class DriveConstants {
+    public static final boolean kGyroReversed = false;
+    public static final double ksVolts = -1;
+    public static final double kvVoltSecondsPerMeter = -1;
+    public static final double kaVoltSecondsSquaredPerMeter = -1;
+    public static final double kTrackwidthMeters = -1;
+    public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(
+        kTrackwidthMeters);
+    public static final double kPDriveVel = -1;
+  }
+
+  public static class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = -1;
+    public static final double kMaxAccelerationMetersPerSecondSquared = -1;
+    public static final double kRamseteB = -1;
+    public static final double kRamseteZeta = -1;
+  }
+
+  public static class PID {
+    public static class Flywheel {
+      public static final double P = 0;
+      public static final double I = 0;
+      public static final double D = 0;
     }
 
-    public static class Input {
+    public static class Hood {
+      public static final double P = 0;
+      public static final double I = 0;
+      public static final double D = 0;
     }
 
-    public static class HumanInput {
-        public static class Driver {
-            public static CustomXbox xbox;
-        }
+    public static class Drive {
+    }
 
-        public static class Operator {
-            public static CustomJoystick joystick;
-        }
+    public static class Turn {
     }
 
-    public RobotMap() {
-        HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController);
-        HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick);
+  }
+
+  public static class Component {
+    public static Intake intake;
+    public static Indexer indexer;
+    public static Flywheel flywheel;
+    public static Shooter shooter;
+    public static Hood hood;
+
+    public static SolenoidSubsystem intakeSolenoid;
+    public static SolenoidSubsystem flipperSolenoid;
+    public static SolenoidSubsystem shooterAimSolenoid;
+
+    public static Motor intakeRollerMotor;
+    public static Motor funnelMotor;
+    public static Motor liftBeltMotor;
+    public static Motor runUpBeltMotor;
+    public static Motor flywheelMotorA;
+    public static Motor flywheelMotorB;
+    public static Motor hoodMotor;
+
+    public static CANTalonEncoder flywheelEncoder;
+    public static CANEncoder hoodEncoder;
+  }
+
+  public static class Input {
+    public static CustomDigitalLimitSwitch limitSwitch;
+    public static CustomDigitalLimitSwitch hoodLowLimitSwitch;
+    public static CustomDigitalLimitSwitch hoodHighLimitSwitch;
+  }
+
+  public static class HumanInput {
+    public static class Driver {
+      public static CustomXbox xbox;
+    }
 
+    public static class Operator {
+      public static CustomJoystick joystick;
     }
+  }
+
+  public RobotMap() {
+    HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController);
+    HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick);
+
+    Component.intakeSolenoid = new SolenoidSubsystem(Port.Pneumatics.INTAKE_SOLENOID.buildDoubleSolenoid());
+    Component.flipperSolenoid = new SolenoidSubsystem(Port.Pneumatics.FLIPPER_SOLENOID.buildDoubleSolenoid());
+    Component.shooterAimSolenoid = new SolenoidSubsystem(Port.Pneumatics.SHOOTER_AIM_SOLENOID.buildDoubleSolenoid());
+
+    // TODO: FIX MOTOR TYPES
+    Component.intakeRollerMotor = new Motor("intakeRollerMotor", new CANTalonSRX(Port.CANMotor.INTAKE_ROLLER_MOTOR));
+    Component.funnelMotor = new Motor("funnelMotor", new CANTalonSRX(Port.CANMotor.INTAKE_FUNNEL_MOTOR));
+    Component.liftBeltMotor = new Motor("liftBeltMotor", new CANTalonSRX(Port.CANMotor.LIFT_BELT_MOTOR));
+    Component.runUpBeltMotor = new Motor("runUpBeltMotor", new CANTalonSRX(Port.CANMotor.RUN_UP_BELT_MOTOR));
+    CANTalonFX flywheelATalon = new CANTalonFX(Port.CANMotor.FLYWHEEL_MOTOR_A); //TODO: Nicer way to do this?
+    Component.flywheelMotorA = new Motor("flywheelMotorA", flywheelATalon);
+    Component.flywheelMotorB = new Motor("flywheelMotorB", new CANTalonFX(Port.CANMotor.FLYWHEEL_MOTOR_B));
+    Component.hoodMotor = new Motor("hoodMotor", new ContinuousServoController(Port.PWM.HOOD_MOTOR));
+
+    Component.intake = new Intake(Component.intakeRollerMotor, Component.funnelMotor, Component.intakeSolenoid);
+    Component.indexer = new Indexer(Component.liftBeltMotor, Component.flipperSolenoid, Input.limitSwitch);
+
+    Component.flywheelEncoder = new CANTalonEncoder(flywheelATalon, Metrics.Flywheel.ROTATIONS_PER_TICK);
+    Component.flywheel = new Flywheel(
+        new CustomPIDController(PID.Flywheel.P, PID.Flywheel.I, PID.Flywheel.D, Component.flywheelEncoder));
+
+    Component.hoodEncoder = new CANEncoder(Port.CAN.HOOD_ENCODER);
+    Component.hood = new Hood(Component.hoodMotor, Component.hoodEncoder, Input.hoodLowLimitSwitch, Input.hoodHighLimitSwitch); //TODO: Remove this redundancy.
+    Component.shooter = new Shooter(Component.flywheel, Component.shooterAimSolenoid, Component.hood);
+  }
 }

src/main/java/org/usfirst/frc4904/robot/commands/BoxShot.java

@@ -0,0 +1,22 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.commands.Shoot;
+import org.usfirst.frc4904.robot.subsystems.Indexer;
+import org.usfirst.frc4904.robot.subsystems.Shooter;
+
+public class BoxShot extends Shoot {
+  protected static final double DEFAULT_BOXSHOT_SPEED = 0.0; // TODO: Untested variables
+
+  /**
+   * Run the Shoot command for a box shot.
+   * 
+   * 
+   * @param indexer The indexer subsystem
+   * @param shooter The shooter subsystem
+   * @param speed   The target speed of the flywheel
+   */
+  public BoxShot(Indexer indexer, Shooter shooter) {
+    super(indexer, shooter, DEFAULT_BOXSHOT_SPEED);
+    setName("BoxShot");
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/CloseIndexer.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.Indexer;
+import org.usfirst.frc4904.standard.commands.solenoid.SolenoidExtend;
+
+public class CloseIndexer extends SolenoidExtend {
+  /**
+   * Retract the indexer solenoid to close the indexer flippers.
+   * 
+   * @param indexer The indexer to manipulate
+   */
+  public CloseIndexer(Indexer indexer) {
+    super("CloseIndexer", indexer.flippers);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/ExtendIntake.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.Intake;
+import org.usfirst.frc4904.standard.commands.solenoid.SolenoidExtend;
+
+public class ExtendIntake extends SolenoidExtend {
+  /**
+   * Extend the intake solenoid to deploy the intake rollers.
+   * 
+   * @param intake The intake to manipulate
+   */
+  public ExtendIntake(Intake intake) {
+    super("ExtendIntake", intake.solenoid);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/FlywheelSpinDown.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class FlywheelSpinDown extends CommandBase {
+  public static final double DEFAULT_IDLE_SPEED = 0.0;
+  protected final Flywheel flywheel;
+
+  /**
+   * Spin down the flywheel
+   * 
+   * @param flywheel The flywheel to manipulate
+   */
+  FlywheelSpinDown(Flywheel flywheel) {
+    super();
+    setName("FlywheelSpinDown");
+    addRequirements(flywheel);
+    this.flywheel = flywheel;
+  }
+
+  @Override
+  public void execute() {
+    flywheel.setSpeed(DEFAULT_IDLE_SPEED);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/FlywheelSpinUp.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+import org.usfirst.frc4904.standard.commands.motor.MotorVelocitySet;
+public class FlywheelSpinUp extends MotorVelocitySet {
+  public static final double DEFAULT_FLYWHEEL_SPEED = 0.0; // TODO: This is an untested value
+
+  /**
+   * Spin up the flywheel to a speed
+   * 
+   * @param flywheel The flywheel to manipulate
+   * @param speed    The speed to spin the flywheel up to
+   */
+  FlywheelSpinUp(Flywheel flywheel, double speed) {
+    super("FlywheelSpinUp", flywheel, speed);
+  }
+
+  /**
+   * Spin up the flywheel to the default speed
+   * 
+   * @param flywheel The flywheel to manipulate
+   */
+  FlywheelSpinUp(Flywheel flywheel) {
+    this(flywheel, DEFAULT_FLYWHEEL_SPEED);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/HoodZeroConstant.java

@@ -0,0 +1,28 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.Hood;
+import org.usfirst.frc4904.standard.commands.motor.MotorConstant;
+
+public class HoodZeroConstant extends MotorConstant {
+    protected final boolean limitType;
+    protected final Hood hood;
+
+    public HoodZeroConstant(String name, Hood hood, double motorSpeed, boolean limitType) {
+        super(name, hood.getServo(), motorSpeed);
+        this.hood = hood;
+        this.limitType = limitType;
+    }
+
+    public HoodZeroConstant(Hood hood, double motorspeed, boolean limitType) {
+        this("HoodZeroConstant", hood, motorspeed, limitType);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (!interrupted) {
+            super.motor.set(0.0);
+            hood.setLimit(limitType);
+        }
+    }
+
+}