Merge pull request #62 from frc3197/detroit-prep

Tweaking and rebuild

src/main/java/org/team3197/frc2019/robot/OI.java

@@ -29,13 +29,14 @@ public class OI {
         private static POVButton secondaryDPadDown = new POVButton(secondary, 180);
         private static POVButton secondaryDPadLeft = new POVButton(secondary, 270);
 
-        private static JoystickButton driverA = new JoystickButton(driver, 1);
-        private static JoystickButton driverB = new JoystickButton(driver, 2);
-        private static JoystickButton driverX = new JoystickButton(driver, 3);
-        private static JoystickButton driverY = new JoystickButton(driver, 4);
+        // private static JoystickButton driverA = new JoystickButton(driver, 1);
+        // private static JoystickButton driverB = new JoystickButton(driver, 2);
+        // private static JoystickButton driverX = new JoystickButton(driver, 3);
+        // private static JoystickButton driverY = new JoystickButton(driver, 4);
 
         private static JoystickButton secondaryA = new JoystickButton(secondary, 1);
-        private static JoystickButton secondaryB = new JoystickButton(secondary, 2);
+        // // private static JoystickButton secondaryB = new JoystickButton(secondary,
+        // 2);
         private static JoystickButton secondaryX = new JoystickButton(secondary, 3);
         private static JoystickButton secondaryY = new JoystickButton(secondary, 4);
 
@@ -46,24 +47,20 @@ public class OI {
 
         static {
 
-                // driverA.whenPressed(Robot.driveTrain.changeDriveMode);
+                secondaryX.whenPressed(Robot.wrist.toggleGyro);
 
-                // driverY.whileHeld(Robot.autoClimb);
-
-                secondaryX.whenPressed(Robot.arm.toggleGyro);
-
-                secondaryY.whenPressed(Robot.arm.resetEncoder);
+                secondaryY.whenPressed(Robot.wrist.resetGyro);
+                secondaryY.whenPressed(Robot.elbow.resetEncoder);
                 secondaryY.whenPressed(Robot.elevator.reset);
-                secondaryY.whenPressed(Robot.arm.resetGyro);
 
                 /**
                  * If the right bumper is pushed, then the cargo intake will move. If the right
                  * bumper is not held, then the hatch mech will be in position.
                  */
-                secondaryDPadUp.whileHeld(new LevelThree(Robot.elevator, Robot.arm, secondaryA));
-                secondaryDPadRight.whileHeld(new LevelTwo(Robot.elevator, Robot.arm, secondaryA));
-                secondaryDPadDown.whileHeld(new LevelOne(Robot.elevator, Robot.arm, secondaryA));
-                secondaryDPadLeft.whileHeld(new Cargo(Robot.elevator, Robot.arm, secondaryA));
+                secondaryDPadUp.whileHeld(new LevelThree(Robot.elevator, Robot.elbow, secondaryA));
+                secondaryDPadRight.whileHeld(new LevelTwo(Robot.elevator, Robot.elbow, secondaryA));
+                secondaryDPadDown.whileHeld(new LevelOne(Robot.elevator, Robot.elbow, secondaryA));
+                secondaryDPadLeft.whileHeld(new Cargo(Robot.elevator, Robot.elbow, secondaryA));
         }
 
         public static double arcadeDriveY() {

src/main/java/org/team3197/frc2019/robot/Robot.java

@@ -1,17 +1,18 @@
 package org.team3197.frc2019.robot;
 
-import org.team3197.frc2019.robot.commands.test.DriveTrainTest;
-import org.team3197.frc2019.robot.subsystems.Arm;
 import org.team3197.frc2019.robot.subsystems.Climber;
 import org.team3197.frc2019.robot.subsystems.DriveTrain;
+import org.team3197.frc2019.robot.subsystems.Elbow;
 import org.team3197.frc2019.robot.subsystems.Elevator;
 import org.team3197.frc2019.robot.subsystems.Erector;
 import org.team3197.frc2019.robot.subsystems.Hatch;
 import org.team3197.frc2019.robot.subsystems.Intake;
+import org.team3197.frc2019.robot.subsystems.Wrist;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -21,16 +22,14 @@ public class Robot extends TimedRobot {
 
   public static final DriveTrain driveTrain = new DriveTrain();
   public static final Elevator elevator = new Elevator();
-  public static final Arm arm = new Arm();
+  public static final Elbow elbow = new Elbow();
+  public static final Wrist wrist = new Wrist();
   public static final Intake manipulator = new Intake();
   public static final Hatch hatch = new Hatch();
   public static final Erector erector = new Erector();
   public static final Climber climber = new Climber();
 
-  // public static final GyroClimb autoClimb = new GyroClimb(climber);
-
-  // public static final PowerDistributionPanel pdp = new
-  // PowerDistributionPanel();
+  public static final PowerDistributionPanel pdp = new PowerDistributionPanel();
 
   public static final NetworkTableInstance ntInst = NetworkTableInstance.getDefault();
 
@@ -46,7 +45,6 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     SmartDashboard.putNumber("Time", DriverStation.getInstance().getMatchTime());
-    SmartDashboard.putNumber("verticalGyroSpeedReal", climber.getAngle());
   }
 
   @Override
@@ -81,7 +79,6 @@ public void teleopPeriodic() {
   @Override
   public void testInit() {
     reset();
-    Scheduler.getInstance().add(new DriveTrainTest(driveTrain));
   }
 
   @Override
@@ -90,12 +87,11 @@ public void testPeriodic() {
   }
 
   private void reset() {
-    arm.resetGyro.start();
-    climber.resetGyro.start();
+    wrist.resetGyro.start();
     erector.resetPID.start();
     if (resetEncoders) {
       elevator.reset.start();
-      arm.resetEncoder.start();
+      elbow.resetEncoder.start();
       resetEncoders = false;
     }
   }

src/main/java/org/team3197/frc2019/robot/RobotMap.java

@@ -95,10 +95,6 @@ private MaxSpeeds(double speed) {
 
   public static final double elbowDegreeSensitivity = 0.15;
 
-  public static final double elevatorExponent = 0.5;
-
-  public static final double elbowExponent = 0.5;
-
   public static final double elevatorSpeedMultiplier = 0.5;
 
   public static final double elbowSpeedMultiplier = 0.5;

src/main/java/org/team3197/frc2019/robot/commands/ElevateToPreset.java

@@ -65,8 +65,7 @@ private double getElevatorSpeed() {
     double error = elevator.getEncoderPosition() - currentTarget.pos;
     finished = Math.abs(error) < RobotMap.elevatorPresetThreshold;
 
-    double speed = -RobotMap.elevatorDegreeSensitivity
-        * Math.copySign(Math.pow(Math.abs(error), RobotMap.elevatorExponent), error);
+    double speed = -RobotMap.elevatorDegreeSensitivity * error;
     return speed;
   }
 }

src/main/java/org/team3197/frc2019/robot/commands/ArticulateToPreset.java → src/main/java/org/team3197/frc2019/robot/commands/ExtendToPreset.java

@@ -2,33 +2,32 @@
 
 import org.team3197.frc2019.robot.RobotMap;
 import org.team3197.frc2019.robot.RobotMap.ElbowPreset;
-import org.team3197.frc2019.robot.RobotMap.MaxSpeeds;
-import org.team3197.frc2019.robot.subsystems.Arm;
+import org.team3197.frc2019.robot.subsystems.Elbow;
 
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import edu.wpi.first.wpilibj.command.Command;
 
-public class ArticulateToPreset extends Command {
+public class ExtendToPreset extends Command {
 
   private final ElbowPreset target;
   private final ElbowPreset targetWithTrigger;
 
   private final Trigger toggle;
 
-  private Arm arm;
+  private Elbow elbow;
 
   private boolean finished;
 
   /**
    * Sets the value of the preset to the one that is intended to be moved to
    */
-  public ArticulateToPreset(ElbowPreset target, ElbowPreset targetWithTrigger, Trigger toggle, Arm arm) {
+  public ExtendToPreset(ElbowPreset target, ElbowPreset targetWithTrigger, Trigger toggle, Elbow elbow) {
     super();
-    requires(arm);
+    requires(elbow);
     this.target = target;
     this.targetWithTrigger = targetWithTrigger;
     this.toggle = toggle;
-    this.arm = arm;
+    this.elbow = elbow;
     finished = false;
   }
 
@@ -39,10 +38,7 @@ public ArticulateToPreset(ElbowPreset target, ElbowPreset targetWithTrigger, Tri
   @Override
   protected void execute() {
     double elbowSpeed = getElbowSpeed();
-
-    // TODO adjust the speeds here
-    arm.elbow(elbowSpeed * MaxSpeeds.kArm.forwardSpeed);
-    arm.wrist(0);
+    elbow.drive(elbowSpeed, true);
   }
 
   @Override
@@ -52,8 +48,7 @@ protected boolean isFinished() {
 
   @Override
   protected void end() {
-    arm.elbow(0);
-    arm.wrist(0);
+    elbow.drive(0, true);
   }
 
   /**
@@ -64,11 +59,10 @@ protected void end() {
   private double getElbowSpeed() {
     ElbowPreset currentTarget = (toggle.get()) ? targetWithTrigger : target;
 
-    double error = arm.getElbowEncoderPosition() - currentTarget.elbowPos;
+    double error = elbow.getElbowEncoderPosition() - currentTarget.elbowPos;
     finished = Math.abs(error) < RobotMap.elbowPresetThreshold;
 
-    double speed = -RobotMap.elbowDegreeSensitivity
-        * Math.copySign(Math.pow(Math.abs(error), RobotMap.elbowExponent), error);
+    double speed = -RobotMap.elbowDegreeSensitivity * error;
     return speed;
   }
 }