Merge pull request #33 from pietroglyph/master

Got Command Group Values Working

src/org/usfirst/frc/team4915/steamworks/OI.java

@@ -103,7 +103,7 @@ private void initClimberOI()
     private void initDrivetrainOI()
     {
         m_robot.getDrivetrain().setDriveStick(m_driveStick);
-        m_turnIMUStart.whenPressed(new TurnDegreesIMU(m_robot.getDrivetrain(), 45));
+        m_turnIMUStart.whenPressed(new TurnSequenceCommandGroup(m_robot.getDrivetrain()));
         m_driveDistance.whenPressed(new DriveDistanceCmd(m_robot.getDrivetrain(), 36));; // needs tweaking!
         m_driveDistancePID.whenPressed(new DriveDistancePIDCmd(m_robot.getDrivetrain(), 57.3));; // needs tweaking!
     }

src/org/usfirst/frc/team4915/steamworks/commandgroups/TurnSequenceCommandGroup.java

@@ -16,26 +16,11 @@ public class TurnSequenceCommandGroup extends CommandGroup
 
     public TurnSequenceCommandGroup(Drivetrain drivetrain)
     {
-        // Add Commands here:
-        // e.g. addSequential(new Command1());
-        //      addSequential(new Command2());
-        // these will run in order.
         m_drivetrain = drivetrain;
-        // To run multiple commands at the same time,
-        // use addParallel()
-        // e.g. addParallel(new Command1());
-        //      addSequential(new Command2());
-        // Command1 and Command2 will run in parallel.
-
-        // A command group will require all of the subsystems that each member
-        // would require.
-        // e.g. if Command1 requires chassis, and Command2 requires arm,
-        // a CommandGroup containing them would require both the chassis and the
-        // arm.
-        addSequential(new TurnDegreesIMU(m_drivetrain, 45));
-        addSequential(new TurnDegreesIMU(m_drivetrain, -45));
-        addSequential(new DriveDistancePIDCmd(m_drivetrain, 57.3));
-        addSequential(new TurnDegreesIMU(m_drivetrain, 180));
-
+        addSequential(new DriveDistancePIDCmd(m_drivetrain, 25)); // Calculated by circumscribing a circle around the robot and getting it's radius and adding 4 for some leeway
+        addSequential(new TurnDegreesIMU(m_drivetrain, -90));
+        addSequential(new DriveDistancePIDCmd(m_drivetrain, 103));
+        addSequential(new TurnDegreesIMU(m_drivetrain, -123));
+        addSequential(new DriveDistancePIDCmd(m_drivetrain, 3));
     }
 }

src/org/usfirst/frc/team4915/steamworks/commands/DriveDistancePIDCmd.java

@@ -35,7 +35,7 @@ public void initialize()
     {
         m_drivetrain.m_logger.info("DriveDistancePIDCmd initialize");
         m_drivetrain.setControlMode(TalonControlMode.PercentVbus, 12.0, -12.0,
-                0.0, 0, 0, 0 /* zero PIDF */);
+                0.0, 0, 0, 0 /* zero PIDF (We're not using Talon PID Control */);
         m_drivetrain.resetPosition();
         m_pidController.reset(); // Reset all of the things that have been passed to the IMU in any previous turns
         m_pidController.setSetpoint(m_revs); // Set the point we want to turn to
@@ -54,7 +54,7 @@ public void execute()
     @Override
     public boolean isFinished()
     {
-        if (m_pidController.onTarget()) 
+        if (m_pidController.onTarget())
         {
             m_drivetrain.m_logger.debug("DriveDistancePIDCmd PID is on target");
         }

src/org/usfirst/frc/team4915/steamworks/commands/TurnDegreesIMU.java

@@ -14,7 +14,7 @@ public class TurnDegreesIMU extends Command
 
     private final Drivetrain m_drivetrain;
     private double m_degrees;
-    private int targetCounter;
+    private int m_targetCounter;
 
     public TurnDegreesIMU(Drivetrain drivetrain, double degrees)
     {
@@ -26,7 +26,7 @@ public TurnDegreesIMU(Drivetrain drivetrain, double degrees)
     @Override
     protected void initialize()
     {
-        targetCounter = 0;
+        m_targetCounter = 0;
         // Will the IMU be initialized by the time we get here?
         m_drivetrain.endIMUTurn();
         m_drivetrain.setControlMode(TalonControlMode.PercentVbus, 12.0, -12.0,
@@ -39,17 +39,16 @@ protected void initialize()
     protected void execute()
     {
         // We shouldn't have to do anything here because all of the PID control stuff runs in another thread
-        m_drivetrain.debugIMU();
     }
 
     @Override
     protected boolean isFinished()
     {
         if (m_drivetrain.isIMUTurnFinished())
         {
-            m_drivetrain.m_logger.debug(targetCounter+"");
-            targetCounter++;
-            if (targetCounter > 20) // Make sure that we're on target for a while
+            m_drivetrain.m_logger.debug(m_targetCounter+"");
+            m_targetCounter++;
+            if (m_targetCounter > 20) // Make sure that we're on target for a while
             {
                 return m_drivetrain.isIMUTurnFinished(); // We're done when the IMU turn is done
             }
@@ -60,7 +59,7 @@ protected boolean isFinished()
         }
         else
         {
-            targetCounter = 0;
+            m_targetCounter = 0;
             return false;
         }
 //        return m_drivetrain.isIMUTurnFinished();

src/org/usfirst/frc/team4915/steamworks/subsystems/Drivetrain.java

@@ -148,7 +148,7 @@ public void pidWrite(double output)
             // Should the numbers below be replace with constants?
             m_turningPIDController.setOutputRange(-1, 1); // Set the output range so that this works with our PercentVbus turning method
             m_turningPIDController.setInputRange(-180, 180); // We do this so that the PIDController takes inputs consistent with our IMU's outputs
-            m_turningPIDController.setAbsoluteTolerance(1); // This is the tolerance for error for reaching our target
+            m_turningPIDController.setAbsoluteTolerance(3); // This is the tolerance for error for reaching our target
 
             // Make a new RobotDrive so we can use built in WPILib functions like ArcadeDrive
             m_robotDrive = new RobotDrive(m_portMasterMotor, m_starboardMasterMotor);
@@ -217,15 +217,8 @@ public void endIMUTurn()
         }
     }
 
-    public void debugIMU()
-    {
-        m_logger.debug("onTarget: " + m_turningPIDController.onTarget() + "heading: " + m_imu.getHeading() + "PID: " + m_turningPIDController.get());
-        System.out.println("I should be outputting debug information.");
-    }
-
     public boolean isIMUTurnFinished()
     {
-        debugIMU();
         return m_turningPIDController.onTarget();
     }