Merge branch 'master' of https://github.com/Team4065/Robot2020Java

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

@@ -6,17 +6,41 @@
 /*----------------------------------------------------------------------------*/
 
 package frc.robot;
-
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 
-/**
- * This is where the robot and the controller inputs are configured CAN IDs must
- * not repeat or else it will result in unexpected behaviors
- */
 public class Constants {
     public static final boolean IS_GYRO_REVERSED = true;
     public static final boolean IS_SPY_ENABLED = true;
 
+
+    //Ramsete
+    public static final double ROBOT_TRACKWIDTH = Double.NaN;
+    public static final double RAMSETE_B = Double.NaN;
+    public static final double RAMSETE_ZETA =  Double.NaN;
+    public static final double KS_VOLTS = Double.NaN;
+    public static final double KV_VOLT_SECONDS_PER_METER = Double.NaN;
+    public static final double KA_VOLT_SECONDS_SQUARED_PER_METER = Double.NaN;
+    public static final DifferentialDriveKinematics DIFFERENTIAL_DRIVE_KINEMATICS = new DifferentialDriveKinematics(ROBOT_TRACKWIDTH);
+    public static final double KP_DRIVE_VEL = Double.NaN;
+
+    //Lift
+	public static final int LIFT_UP = 0;//up and down are pneumatic ids
+	public static final int LIFT_DOWN = 0;
+    public static final int LEFT_LIFT_MOTOR = 0;
+    public static final int RIGHT_LIFT_MOTOR = 0;
+    public static final double MAX_LIFT_HEIGHT = 0;//units in of rotations of motor
+
+    //Drivetrain
+	public static final int LEFT_DRIVETRAIN_MASTER = 0;
+	public static final int RIGHT_DRIVETRAIN_MASTER = 0;
+	public static final int[] LEFT_DRIVTRAIN_SLAVES = new int[]{0};
+    public static final int[] RIGHT_DRIVETRAIN_SLAVES = new int[]{0};
+
+    //Intake
+	public static final int INTAKE_MOTOR = 0;
+	public static final int INTAKE_DEPLOY = 0;//deploy and retract are pneumatic ids
+	public static final int INTAKE_RETRACT = 0;
+
     //Shooter
     public static final int SHOOTER_LEFT_MASTER_ID = 0;
     public static final int SHOOTER_RIGHT_SLAVE_ID = 0;
@@ -31,26 +55,5 @@ public class Constants {
     public static final int SERIALIZER_MOTOR_ID = 0;
 
     //Kicker
-    public static final int KICKER_MOTOR_ID = 0;
-
-    //Ramsete
-    public static final double ROBOT_TRACKWIDTH = Double.NaN;
-    public static final double RAMSETE_B = Double.NaN;
-    public static final double RAMSETE_ZETA =  Double.NaN;
-    public static final double KS_VOLTS = Double.NaN;
-    public static final double KV_VOLT_SECONDS_PER_METER = Double.NaN;
-    public static final double KA_VOLT_SECONDS_SQUARED_PER_METER = Double.NaN;
-    public static final DifferentialDriveKinematics DIFFERENTIAL_DRIVE_KINEMATICS = new DifferentialDriveKinematics(ROBOT_TRACKWIDTH);
-    public static final double KP_DRIVE_VEL = Double.NaN;
-
-    //Drivetrain
-    public static final int DRIVETRAIN_FRONT_LEFT_ID = 0;
-    public static final int DRIVETRAIN_FRONT_RIGHT_ID = 0;
-    public static final int DRIVETRAIN_BACK_LEFT_ID = 0;
-    public static final int DRIVETRAIN_BACK_RIGHT_ID = 0;
-
-    //Intake
-    public static final int INTAKE_MOTOR_ID = 0;
-    public static final int INTAKE_DEPLOY_ID = 0;
-    public static final int INTAKE_RETRACT_ID = 0;
+    public static final int KICKER_MOTOR_ID = 1;
 }

src/main/java/frc/robot/RobotContainer.java

@@ -7,23 +7,27 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Utility.Motor;
-import frc.robot.Utility.Motor.MotorType;
-import frc.robot.commands.ExampleCommand;
-
-
-
-
-
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
-import frc.robot.commands.Shoot;
+import frc.robot.commands.ExampleCommand;
+import frc.robot.commands.Drivetrain.ArcadeDrive;
+import frc.robot.commands.Intake.IntakeDeploy;
+import frc.robot.commands.Intake.IntakeSpit;
+import frc.robot.commands.Intake.IntakeSuck;
+import frc.robot.commands.Lift.LiftDeploy;
+import frc.robot.commands.Lift.LiftDown;
+import frc.robot.commands.Lift.LiftRetract;
+import frc.robot.commands.Lift.LiftUp;
 import frc.robot.subsystems.DifferentialDrivetrain;
-import frc.robot.subsystems.Kicker;
-import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Lift;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.Utility.Motor;
+import frc.robot.Utility.Motor.MotorType;
 
-//mport edu.wpi.first.wpilibj2.command.Command;
+//TODO configure motor feedforwards
 
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -40,8 +44,30 @@ public class RobotContainer {
 
 
 
+  Joystick m_controller = new Joystick(0);
+
+  // The robot's subsystems and commands are defined here...
+  private final DifferentialDrivetrain m_drivetrain = new DifferentialDrivetrain(
+    1/*wheel diameter*/,
+    new Motor(Constants.LEFT_DRIVETRAIN_MASTER, MotorType.CANSparkMax),
+    new Motor(Constants.RIGHT_DRIVETRAIN_MASTER, MotorType.CANSparkMax),
+    new Motor[]{new Motor(Constants.LEFT_DRIVTRAIN_SLAVES[0], MotorType.CANSparkMax)},
+    new Motor[]{new Motor(Constants.RIGHT_DRIVETRAIN_SLAVES[0], MotorType.CANSparkMax)}
+  );
+
+  private final Lift m_lift = new Lift();
+  private final Intake m_intake = new Intake();
+
+  private final ExampleCommand m_autoCommand = new ExampleCommand();
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    //m_drivetrain.configFeedforward(kS, kV, kA); Do this
+    //m_drivetrain.configRotationFeedForward(kS, kV, kA);Do this
+
+    m_drivetrain.setDefaultCommand(new ArcadeDrive(m_drivetrain, m_controller, 1, Math.PI / 2));
+    m_intake.setDefaultCommand(new IntakeDeploy(m_intake));//so it deploys on start
+    // Configure the button bindings
     configureButtonBindings();
   }
 
@@ -52,10 +78,16 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    new JoystickButton(m_controller, 0/* button label */).whileHeld(new LiftUp(m_lift));
+    new JoystickButton(m_controller, 0/* button label */).whileHeld(new LiftDown(m_lift));
+    new JoystickButton(m_controller, 0/* button label */).whenPressed(new LiftDeploy(m_lift));
+    new JoystickButton(m_controller, 0/* button label */).whenPressed(new LiftRetract(m_lift));
 
+    new JoystickButton(m_controller, 0/* button label */).whileHeld(new IntakeSuck(m_intake));
+    new JoystickButton(m_controller, 0/* button label */).whileHeld(new IntakeSpit(m_intake));
+
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/Utility/Motor.java

@@ -69,7 +69,7 @@ public Motor(int id, MotorType motorType) {
     }
 
     public void set(ControlMode controlMode, double value) {
-        if(m_isFeedforwardConfigured && controlMode == ControlMode.Velocity){
+        if(!m_isFeedforwardConfigured && controlMode == ControlMode.Velocity){
             controlMode = ControlMode.PercentOutput;
             value = 0;
             System.out.println("You must configure feedforward to use Velocity control.");
@@ -231,8 +231,7 @@ public void follow(Motor master){
     }
 
     /**
-     * If motor is a follower, then this does nothing.
-     * Set follower inversion from master when setting master.
+     * Does nothing if it is a follower
      * @param isInverted
      */
     public void setInverted(boolean isInverted){

src/main/java/frc/robot/Utility/PathLoader.java

@@ -35,4 +35,4 @@ public PathLoader(String pathDirectory){
     public Trajectory getTrajectory(){
         return trajectory;
     }
-}
+}

src/main/java/frc/robot/Utility/Vector3.java

@@ -266,4 +266,4 @@ public static Vector3 rotate(Vector3 vector, Vector3 axis, double angle){
         Vector3 _axis = axis.normalized();
         return vector.mult(Math.cos(Math.toRadians(angle))).add(Vector3.cross(vector, _axis).mult(Math.sin(Math.toRadians(angle)))).add(_axis.mult(Vector3.dot(_axis, vector) * (1 - Math.cos(Math.toRadians(angle)))));
     }
-}
+}

src/main/java/frc/robot/commands/Drivetrain/CharacterizeDrivetrain.java

@@ -8,6 +8,13 @@
 import frc.robot.subsystems.DifferentialDrivetrain;
 import frc.robot.subsystems.DifferentialDrivetrain.ControlMode;
 
+
+/**
+ * To use run the command.
+ * Copy the output into desmos and use regression to find the variables of the equation given at 
+ * https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-characterization/introduction.html
+ */
+
 public class CharacterizeDrivetrain extends CommandBase {
   DifferentialDrivetrain m_drivetrain;
   double m_targetVoltage = 0;

src/main/java/frc/robot/commands/Drivetrain/CharacterizeRotation.java

@@ -9,6 +9,12 @@
 import frc.robot.subsystems.DifferentialDrivetrain;
 import frc.robot.subsystems.DifferentialDrivetrain.ControlMode;
 
+/**
+ * To use run the command.
+ * Copy the output into desmos and use regression to find the variables of the equation given at 
+ * https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-characterization/introduction.html
+ */
+
 public class CharacterizeRotation extends CommandBase {
   DifferentialDrivetrain m_drivetrain;
   double targetVelocity = 0;

src/main/java/frc/robot/commands/ExampleCommand.java

@@ -7,9 +7,14 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class ExampleCommand extends CommandBase {
-  /** Creates a new ExampleCommand. */
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
   public ExampleCommand() {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements();
   }
 
   // Called when the command is initially scheduled.