Implemented lift

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

@@ -24,36 +24,27 @@ public class Constants {
     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
+	public static final int LIFT_UP = 6;//up and down are pneumatic ids
+	public static final int LIFT_DOWN = 2;
+    public static final int LEFT_LIFT_MOTOR = 8;
+    public static final int RIGHT_LIFT_MOTOR = 6;
+    public static final double MAX_LIFT_HEIGHT = 549;//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};
+	public static final int LEFT_DRIVETRAIN_MASTER = 1;
+	public static final int RIGHT_DRIVETRAIN_MASTER = 3;
+	public static final int[] LEFT_DRIVTRAIN_SLAVES = new int[]{2};
+    public static final int[] RIGHT_DRIVETRAIN_SLAVES = new int[]{4};
 
     //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;
-    public static final double SHOOTER_KS = 0;
-    public static final double SHOOTER_KV = 0;
-    public static final double SHOOTER_KA = 0;
-    public static final int SPIN_UP_RPM = 0;
-
-
-    //Feeder 
-    public static final int FEEDER_MOTOR_ID = 0;
-    public static final int SERIALIZER_MOTOR_ID = 0;
+	public static final int INTAKE_DEPLOY = 7;//deploy and retract are pneumatic ids
+    public static final int INTAKE_RETRACT = 3;
+
+    //v belt id = 7
+    //feeder id = 9
+    //kicker id = 11
 
-    //Kicker
-    public static final int KICKER_MOTOR_ID = 1;
+    //shooter left id = 15
+    //shooter right id = 16
 }

src/main/java/frc/robot/Robot.java

@@ -60,6 +60,7 @@ public void robotPeriodic() {
    */
   @Override
   public void disabledInit() {
+    m_robotContainer.m_compressor.stop();
   }
 
   @Override
@@ -94,7 +95,9 @@ public void teleopInit() {
     // this line or comment it out.
 
 
-
+    m_robotContainer.m_compressor.start();
+    //m_robotContainer.
+
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

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

@@ -7,17 +7,20 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.Drivetrain.ArcadeDrive;
+import frc.robot.commands.Drivetrain.CharacterizeDrivetrain;
 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.LiftToHeight;
 import frc.robot.commands.Lift.LiftUp;
 import frc.robot.subsystems.DifferentialDrivetrain;
 import frc.robot.subsystems.Intake;
@@ -36,19 +39,13 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  DifferentialDrivetrain m_drivetrain = new DifferentialDrivetrain(
-    0.154,
-    new Motor(Constants.DRIVETRAIN_FRONT_LEFT_ID, MotorType.TalonFX), new Motor(Constants.DRIVETRAIN_FRONT_RIGHT_ID, MotorType.TalonFX),
-    new Motor[]{new Motor(Constants.DRIVETRAIN_BACK_LEFT_ID, MotorType.TalonFX)}, new Motor[]{new Motor(Constants.DRIVETRAIN_BACK_RIGHT_ID, MotorType.TalonFX)}
-  );
-
-
-
+  Compressor m_compressor = new Compressor();
   Joystick m_controller = new Joystick(0);
+  Joystick m_buttonbox = new Joystick(1);
 
   // The robot's subsystems and commands are defined here...
   private final DifferentialDrivetrain m_drivetrain = new DifferentialDrivetrain(
-    1/*wheel diameter*/,
+    0.159/*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)},
@@ -62,11 +59,14 @@ public class RobotContainer {
 
   /** 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.configFeedforwardSided(
+      -0.189290699848, -0.293030735327, -0.000429988610686,
+      -0.170613477504, -0.304934237999, 0.0020575793998); 
+    //m_drivetrain.configRotationFeedForward(kS, kV, kA);Do this
+    //m_lift.setDefaultCommand(new LiftRetract(m_lift));
     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
+    //m_intake.setDefaultCommand(new IntakeDeploy(m_intake));//so it deploys on start
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -78,13 +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));
+
+    new JoystickButton(m_buttonbox, 3/* button label */).whileHeld(new LiftUp(m_lift));
+    new JoystickButton(m_buttonbox, 4/* button label */).whileHeld(new LiftDown(m_lift));
+    new JoystickButton(m_buttonbox, 1/* button label */).whenPressed(new LiftDeploy(m_lift));
+    new JoystickButton(m_buttonbox, 2/* button label */).whenPressed(new LiftRetract(m_lift));
+    new JoystickButton(m_buttonbox, 5).whenPressed(new LiftToHeight(m_lift, 1));
+    new JoystickButton(m_buttonbox, 6).whenPressed(new LiftToHeight(m_lift, 0));
+//
+    //new JoystickButton(m_controller, 0/* button label */).whileHeld(new IntakeSuck(m_intake));
+    //new JoystickButton(m_controller, 0/* button label */).whileHeld(new IntakeSpit(m_intake));
 
   }
 
@@ -94,6 +97,7 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return new ExampleCommand();
+    return new CharacterizeDrivetrain(m_drivetrain);
+    //return new ExampleCommand();
   }
 }

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

@@ -42,15 +42,16 @@ public void execute() {
     m_drivetrain.setLeftTarget(m_targetVoltage);
     m_drivetrain.setRightTarget(m_targetVoltage);
     m_targetVoltage += 0.01;
-    System.out.print(m_targetVoltage);
+    System.out.printf("%f", m_targetVoltage);
     System.out.print(",");
-    System.out.print(m_drivetrain.getLeftVelocity());
+    System.out.printf("%f", m_drivetrain.getLeftVelocity());
     System.out.print(",");
-    System.out.print((m_drivetrain.getLeftVelocity() - m_pastLeftVel) / (20.0 / 1000.0));
+    System.out.printf("%f", (m_drivetrain.getLeftVelocity() - m_pastLeftVel) / (20.0 / 1000.0));
     System.out.print(",");
-    System.out.print(m_drivetrain.getRightVelocity());
+    System.out.printf("%f", m_drivetrain.getRightVelocity());
     System.out.print(",");
-    System.out.println((m_drivetrain.getRightVelocity() - m_pastRightVel) / (20.0 / 1000.0));
+    System.out.printf("%f", (m_drivetrain.getRightVelocity() - m_pastRightVel) / (20.0 / 1000.0));
+    System.out.println();
 
     m_pastLeftVel = m_drivetrain.getLeftVelocity();
     m_pastRightVel = m_drivetrain.getRightVelocity();

src/main/java/frc/robot/commands/Lift/LiftDeploy.java

@@ -23,7 +23,9 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_lift.deploy();
+    if(m_lift.getHeight() <= 0.02){
+      m_lift.deploy();
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Lift/LiftDown.java

@@ -24,9 +24,6 @@ public void initialize() {}
   @Override
   public void execute() {
     m_lift.moveDown();
-    System.out.print(-1/*voltage output*/);
-    System.out.print(",");
-    System.out.println(m_lift.getDeltaHeight());
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Lift/LiftRetract.java

@@ -18,13 +18,15 @@ public LiftRetract(Lift lift) {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     //This prevents the bar on the bottom of the lift from crashing into the robot.
-    if(m_lift.getHeight() <= 0.1){
+    if(m_lift.getHeight() <= 0.02){
       m_lift.retract();
     }
   }

src/main/java/frc/robot/commands/Lift/LiftToHeight.java

@@ -0,0 +1,56 @@
+// 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.Lift;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Lift;
+
+public class LiftToHeight extends CommandBase {
+  Lift m_lift;
+  Double m_target;
+  /** Creates a new LiftToHeight. */
+  public LiftToHeight(Lift lift, double percentHeight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(lift);
+    m_lift = lift;
+    if(percentHeight > 0.9){
+      m_target = 0.9;
+    }else{
+      if(percentHeight < 0){
+        m_target = 0.0;
+      }else{
+        m_target = percentHeight;
+      }
+    }
+  }
+
+  // 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() {
+    if(m_lift.getHeight() > m_target){
+      m_lift.moveDown();
+    }
+    if(m_lift.getHeight() < m_target){
+      m_lift.moveUp();
+    }
+    //System.out.println(m_lift.getHeight());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_lift.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return Math.abs(m_lift.getHeight() - m_target) < 0.01;
+  }
+}

src/main/java/frc/robot/commands/Lift/LiftUp.java

@@ -24,9 +24,6 @@ public void initialize() {}
   @Override
   public void execute() {
     m_lift.moveUp();
-    System.out.print(1/*voltage output*/);
-    System.out.print(",");
-    System.out.println(m_lift.getDeltaHeight());
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java

@@ -233,7 +233,7 @@ protected void setRightVelocity(double velocity){
    * @return The velocity of the left side in meters per second
    */
   public double getLeftVelocity(){
-    return -m_leftMaster.getVelocity() * m_wheelDiameter * Math.PI;
+    return -m_leftMaster.getVelocity() /* add gear ratio */ * m_wheelDiameter * Math.PI;
   }
 
   /**

src/main/java/frc/robot/subsystems/Lift.java

@@ -21,13 +21,20 @@ public Lift() {
     m_liftLeft.setInverted(false);
     m_liftLeft.resetEncoder();
     m_liftRight.follow(m_liftLeft);
-
-    //m_liftLeft.configFeedforward(kS, kV, kA); do this
+    m_liftLeft.enableBrakeMode(true);
+    m_liftRight.enableBrakeMode(true);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    if(m_liftLeft.getVelocity() > 0 && this.getHeight() >= 0.9){
+      m_liftLeft.set(ControlMode.PercentOutput, 0);
+    }
+
+    if(m_liftLeft.getVelocity() < 0 && this.getHeight() <= 0.0){
+      m_liftLeft.set(ControlMode.PercentOutput, 0);
+    }
   }
 
   /**
@@ -44,15 +51,15 @@ public double getDeltaHeight(){
 
   public void moveUp(){
     if(this.getHeight() < 0.9){
-      m_liftLeft.set(ControlMode.Voltage, 1);
+      m_liftLeft.set(ControlMode.PercentOutput, 1);
     }else{
       m_liftLeft.set(ControlMode.PercentOutput, 0);
     }
   }
 
   public void moveDown(){
-    if(this.getHeight() > 0.1){
-      m_liftLeft.set(ControlMode.Voltage, -1);
+    if(this.getHeight() > 0){
+      m_liftLeft.set(ControlMode.PercentOutput, -1);
     }else{
       m_liftLeft.set(ControlMode.PercentOutput, 0);
     }