update code

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / Robot.java

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 16b2884f1bf1b4a5c1e5dd6d4893132892f05914..b1ce483f8a528f17f807ad81fead2bb2aa605d5f 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -22,10 +22,10 @@ public class Robot extends IterativeRobot {
     shooter = Shooter.getShooter();
     intake = Intake.getIntake();
 
-    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_P_Val, 0.006);
+    SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0.0011);
+    SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, -0.002);
+    SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0.01);
 
     SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
     SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
@@ -56,6 +56,7 @@ public class Robot extends IterativeRobot {
   // both set to high gear
   @Override
   public void autonomousInit() {
+    System.out.println("AUTON INIT");
     driveTrain.setHighGear();
 
     double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_P_Val,
@@ -90,7 +91,7 @@ public class Robot extends IterativeRobot {
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
-
+    SmartDashboard.putNumber("angle", driveTrain.getAngle());
   }
 
   @Override