From: Eric Sandoval <harpnart@gmail.com>
Date: Sun, 19 Feb 2017 22:02:51 +0000 (-0800)
Subject: update code
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=b5ddf3e2c3d2bc28d108731019840a0561dd775b

update code
---

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 29fc9aa..cae940f 100644
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,7 +1,5 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
 import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
@@ -74,14 +72,6 @@ public class OI {
         Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
     decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
 
-    if (!isClimbing) {
-      toggleWinch.whenPressed(new RunWinchContinuous());
-      isClimbing = true;
-    } else {
-      toggleWinch.whenPressed(new MaintainClimbedPosition());
-      isClimbing = false;
-    }
-
     /*
      * if (!Robot.getDriveTrain().isClimbing()) { toggleWinch.whenPressed(new
      * RunWinchContinuous()); Robot.getDriveTrain().setClimbing(true); } else {
diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 0d73631..6388d0a 100644
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -25,6 +25,8 @@ public class Robot extends IterativeRobot {
     SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0);
     SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0);
     SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, 0);
+    SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0);
+
     SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
     SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
 
@@ -77,11 +79,12 @@ public class Robot extends IterativeRobot {
 
     driveTrain.getDriveController().setName("Drive");
     driveTrain.getGyroController().setName("Gyro");
-    driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
 
+    driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
 
-    Scheduler.getInstance().add(new DriveDistance(setpoint, maxTimeOut));
+    Scheduler.getInstance()
+        .add(new DriveDistance(setpoint, maxTimeOut));
   }
 
   @Override
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
index bd923a2..cb8227f 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -6,6 +6,7 @@ import org.usfirst.frc.team3501.robot.utils.PIDController;
 
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This command makes the robot drive a specified distance using encoders on the
@@ -22,11 +23,13 @@ public class DriveDistance extends Command {
   private Preferences prefs;
   private PIDController driveController;
 
-  private double gyroP;
+  private double driveStraightGyroP;
 
   public DriveDistance(double distance, double maxTimeOut) {
     requires(driveTrain);
     this.target = distance;
+    driveStraightGyroP = SmartDashboard.getNumber(driveTrain.DRIVE_GYRO_P_Val,
+        driveTrain.PID_ERROR);
 
     this.driveController = driveTrain.getDriveController();
     this.driveController.setDoneRange(0.5);
@@ -42,12 +45,18 @@ public class DriveDistance extends Command {
 
   @Override
   protected void execute() {
-    double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+    double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
     double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
 
     double leftDrive = yVal - xVal;
     double rightDrive = yVal + xVal;
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+    System.out.println(
+        "PID VALS: " + driveController.getP() + " " + driveController.getI()
+            + " " + driveController.getD());
+
+    System.out.println(driveTrain.getAvgEncoderDistance());
   }
 
   @Override
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index c832b8f..1196d16 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -21,6 +21,7 @@ public class DriveTrain extends Subsystem {
   public static final String DRIVE_P_Val = "DriveP";
   public static final String DRIVE_I_Val = "DriveI";
   public static final String DRIVE_D_Val = "DriveD";
+  public static final String DRIVE_GYRO_P_Val = "DriveGyroP";
   public static final String DRIVE_TARGET_DIST = "SET_DIST";
   public static final String MAX_TIME_OUT = "MaxTimeOut";
   public static final String GYRO_P_Val = "GyroP";
@@ -30,7 +31,7 @@ public class DriveTrain extends Subsystem {
   public static final int PID_ERROR = -1;
   public static final int TARGET_DISTANCE_ERROR = -1;
 
-  public static final double WHEEL_DIAMETER = 6; // inches
+  public static final double WHEEL_DIAMETER = 4; // inches
   public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
   public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
       / ENCODER_PULSES_PER_REVOLUTION;