Add pid subsystem

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index e092169848cfac5bd21cd26634534a5f6025e701..8f121992ae3b606a0566545abd8730ba9a9b28b3 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -23,24 +23,26 @@ public class Constants {
 
   public static class DriveTrain {
     // Drivetrain Motor Related Ports
-    public static final int FRONT_LEFT = 0;
-    public static final int FRONT_RIGHT = 0;
-    public static final int REAR_LEFT = 0;
-    public static final int REAR_RIGHT = 0;
+    public static final int FRONT_LEFT = 1;
+    public static final int FRONT_RIGHT = 4;
+    public static final int REAR_LEFT = 2;
+    public static final int REAR_RIGHT = 3;
 
     // Encoder related ports
-    public final static int ENCODER_LEFT_A = 3;
-    public final static int ENCODER_LEFT_B = 4;
-    public final static int ENCODER_RIGHT_A = 2;
-    public final static int ENCODER_RIGHT_B = 1;
+    public final static int ENCODER_LEFT_A = 0;
+    public final static int ENCODER_LEFT_B = 1;
+    public final static int ENCODER_RIGHT_A = 9;
+    public final static int ENCODER_RIGHT_B = 8;
 
     private final static double WHEEL_DIAMETER = 6.0; // in inches
     private final static double PULSES_PER_ROTATION = 256; // in pulses
     private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
     private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
-    public final static double INCHES_PER_PULSE = (((Math.PI)
-        * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-        / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+    public static final double INCHES_PER_PULSE =
+        ((3.66 / 5.14) * 6 * Math.PI) / 256;
+
+    public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
+    public static double time = 0;
   }
 
   public static class Scaler {
@@ -77,9 +79,6 @@ public class Constants {
     public static final int POT_CHANNEL = 0;
     public static final double INTAKE_SPEED = 0.5;
     public static final double OUTPUT_SPEED = -0.5;
-    public static final double UP_SPEED = 0.3;
-    public static final double DOWN_SPEED = -0.3;
-    public static final double STOP_SPEED = 0.0;
     public final static double FULL_RANGE = 270.0; // in degrees
     public final static double OFFSET = -135.0; // in degrees
     public static final double ZERO_ANGLE = 0;

diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java
index a57b5ce7efa4aa3f8d02f040652dd450485f3d4a..b7eaeb7cb6ae0c36a7cee9c30b770a26253d5d78 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/MathLib.java
+++ b/src/org/usfirst/frc/team3501/robot/MathLib.java
@@ -57,7 +57,7 @@ public class MathLib {
    *          the largest acceptable value.
    * @return returns value restricted to be within the range [low, high].
    */
-  public static double restrictToRange(double value, double low, double high) {
+  public static double constrain(double value, double low, double high) {
     value = Math.max(value, low);
     value = Math.min(value, high);
     return value;
@@ -65,7 +65,7 @@ public class MathLib {
 
   /***
    * Returns true if val is in the range [low, high]
-   * 
+   *
    * @param val
    *          value to test
    * @param low

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
new file mode 100644 (file)
index 0000000..1eb707c
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -0,0 +1,60 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command will move the robot at the specified speed for the specified
+ * distance.
+ *
+ * post-condition: has driven for the distance and speed specified
+ *
+ * @author Meryem and Avi
+ *
+ */
+public class DriveDistance extends Command {
+  private double maxTimeOut;
+  double distance;
+  int count = 0;
+
+  public DriveDistance(double distance, double maxTimeOut) {
+    requires(Robot.driveTrain);
+    this.maxTimeOut = maxTimeOut;
+    this.distance = distance;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.resetEncoders();
+  }
+
+  @Override
+  protected void execute() {
+    Robot.driveTrain.driveDistance(distance, maxTimeOut);
+
+  }
+
+  @Override
+  protected boolean isFinished() {
+    if (timeSinceInitialized() >= maxTimeOut
+        || Robot.driveTrain
+            .reachedTarget() || Robot.driveTrain.getError() < 1) {
+      System.out.println("time: " + timeSinceInitialized());
+      Constants.DriveTrain.time = timeSinceInitialized();
+      return true;
+    }
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.disable();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java
new file mode 100644 (file)
index 0000000..41e44d3
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class JoystickDrive extends Command {
+
+  public JoystickDrive() {
+    requires(Robot.driveTrain);
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+    // IDK why but the joystick gives positive values for pulling backwards
+    double left = -Robot.oi.leftJoystick.getY();
+    double right = -Robot.oi.rightJoystick.getY();
+    Robot.driveTrain.drive(left, right);
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java
index dc1343948d494575030477fd7b80a2e3467cc87e..675f9a90b3af362e4225a0af3589a188b4dd83d2 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java
@@ -1,30 +1,34 @@
 package org.usfirst.frc.team3501.robot.commands.driving;
 
+import org.usfirst.frc.team3501.robot.Robot;
+
 import edu.wpi.first.wpilibj.command.Command;
 
 public class Stop extends Command {
 
-       public Stop() {
-       }
+  public Stop() {
+  }
 
-       @Override
-       protected void initialize() {
-       }
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.stop();
+  }
 
-       @Override
-       protected void execute() {
-       }
+  @Override
+  protected void execute() {
+  }
 
-       @Override
-       protected boolean isFinished() {
-               return false;
-       }
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
 
-       @Override
-       protected void end() {
-       }
+  @Override
+  protected void end() {
+  }
 
-       @Override
-       protected void interrupted() {
-       }
+  @Override
+  protected void interrupted() {
+    end();
+  }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD
new file mode 100644 (file)
index 0000000..c22bf82
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team3501.robot.commands.intakearm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveIntakeArmUp extends Command {
+
+  public MoveIntakeArmUp() {
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+  }
+
+  @Override
+  protected void interrupted() {
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid
new file mode 100644 (file)
index 0000000..bc7b824
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team3501.robot.commands.intakearm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveIntakeArmUp extends Command {
+       public MoveIntakeArmUp() {
+       }
+
+       @Override
+       protected void initialize() {
+       }
+
+       @Override
+       protected void execute() {
+       }
+
+       @Override
+       protected boolean isFinished() {
+               return false;
+       }
+
+       @Override
+       protected void end() {
+       }
+
+       @Override
+       protected void interrupted() {
+       }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 935b430221d76f22ccf566b5f7910dad27ea1ab8..81f80b92ffa3d7ccd670757b3444273ca8438580 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,59 +1,82 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Lidar;
+import org.usfirst.frc.team3501.robot.GyroLib;
+import org.usfirst.frc.team3501.robot.MathLib;
+import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
-import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.CANTalon;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.RobotDrive;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+
+public class DriveTrain extends PIDSubsystem {
+  private static double pidOutput = 0;
+  private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
+  private int DRIVE_MODE = 1;
+
+  private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
 
-public class DriveTrain extends Subsystem {
-  // Drivetrain related objects
   private Encoder leftEncoder, rightEncoder;
-  public static Lidar lidar;
   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
-  private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
-  // Drivetrain specific constants that relate to the inches per pulse value for
-  // the encoders
-  private final static double WHEEL_DIAMETER = 6.0; // in inches
-  private final static double PULSES_PER_ROTATION = 256; // in pulses
-  private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
-  private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
-  public final static double INCHES_PER_PULSE = (((Math.PI)
-      * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-      / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
-
-  // Drivetrain specific constants that relate to the PID controllers
-  private final static double Kp = 1.0, Ki = 0.0,
-      Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
-
-  public AnalogInput channel;
+  private RobotDrive robotDrive;
+
+  private GyroLib gyro;
 
   public DriveTrain() {
+    super(kp, ki, kd);
+
     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
 
-    lidar = new Lidar(I2C.Port.kOnboard);
+    robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
-    leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
-    rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+
+    leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+    rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+
+    gyro = new GyroLib(I2C.Port.kOnboard, false);
+
+    DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
+    setEncoderPID();
+    this.disable();
+    gyro.start();
 
   }
 
   @Override
   protected void initDefaultCommand() {
+    setDefaultCommand(new JoystickDrive());
+  }
+
+  public void printOutput() {
+    System.out.println("PIDOutput: " + pidOutput);
+  }
+
+  private double getAvgEncoderDistance() {
+    return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
+  }
+
+  public boolean reachedTarget() {
+    if (this.onTarget()) {
+      this.disable();
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public void stop() {
+    drive(0, 0);
   }
 
   public void resetEncoders() {
@@ -61,10 +84,6 @@ public class DriveTrain extends Subsystem {
     rightEncoder.reset();
   }
 
-  public double getLidarDistance() {
-    return lidar.pidGet();
-  }
-
   public double getRightSpeed() {
     return rightEncoder.getRate(); // in inches per second
   }
@@ -85,20 +104,144 @@ public class DriveTrain extends Subsystem {
     return leftEncoder.getDistance(); // in inches
   }
 
-  public double getDistance() {
-    return (getRightDistance() + getLeftDistance()) / 2.0; // in inches
+  public double getError() {
+    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
+      return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
+    else
+      return Math.abs(this.getSetpoint() + getGyroAngle());
   }
 
-  public void stop() {
-    setMotorSpeeds(0, 0);
+  public double getGyroAngle() {
+    return gyro.getRotationZ().getAngle();
+  }
+
+  public void resetGyro() {
+    gyro.reset();
+  }
+
+  public void printEncoder(int i, int n) {
+    if (i % n == 0) {
+      System.out.println("Left: " + this.getLeftDistance());
+      System.out.println("Right: " + this.getRightDistance());
+
+    }
+  }
+
+  public void printGyroOutput() {
+    System.out.println("Gyro Angle" + -this.getGyroAngle());
+  }
+
+  public double getOutput() {
+    return pidOutput;
+  }
+
+  public void updatePID() {
+    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
+      this.getPIDController().setPID(kp, ki, kd);
+    else
+      this.getPIDController().setPID(gp, gd, gi);
+  }
+
+  public CANTalon getFrontLeft() {
+    return frontLeft;
+  }
+
+  public CANTalon getFrontRight() {
+    return frontRight;
+  }
+
+  public CANTalon getRearLeft() {
+    return rearLeft;
+  }
+
+  public CANTalon getRearRight() {
+    return rearRight;
+  }
+
+  public int getMode() {
+    return DRIVE_MODE;
+  }
+
+  @Override
+  protected void usePIDOutput(double output) {
+    double left = 0;
+    double right = 0;
+    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
+      double drift = this.getLeftDistance() - this.getRightDistance();
+      if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
+        output = Math.signum(output) * 0.3;
+      left = output;
+      right = output + drift * kp / 10;
+    }
+    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+      left = output;
+      right = -output;
+    }
+    drive(left, right);
+    pidOutput = output;
   }
 
-  public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
-    // speed passed to right motor is negative because right motor rotates in
-    // opposite direction
-    this.frontLeft.set(leftSpeed);
-    this.frontRight.set(-rightSpeed);
-    this.rearLeft.set(leftSpeed);
-    this.rearRight.set(-rightSpeed);
+  @Override
+  protected double returnPIDInput() {
+    return sensorFeedback();
   }
+
+  private double sensorFeedback() {
+    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
+      return getAvgEncoderDistance();
+    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
+      return -this.getGyroAngle();
+    // counterclockwise is positive on joystick but we want it to be negative
+    else
+      return 0;
+  }
+
+  public void drive(double left, double right) {
+    robotDrive.tankDrive(-left, -right);
+    // dunno why but inverted drive (- values is forward)
+  }
+
+  public void driveDistance(double dist, double maxTimeOut) {
+    dist = MathLib.constrain(dist, -100, 100);
+    setEncoderPID();
+    setSetpoint(dist);
+  }
+
+  public void setEncoderPID() {
+    DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
+    this.updatePID();
+    this.setAbsoluteTolerance(encoderTolerance);
+    this.setOutputRange(-1.0, 1.0);
+    this.setInputRange(-200.0, 200.0);
+    this.enable();
+  }
+
+  private void setGyroPID() {
+    DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
+    this.updatePID();
+    this.getPIDController().setPID(gp, gi, gd);
+
+    this.setAbsoluteTolerance(gyroTolerance);
+    this.setOutputRange(-1.0, 1.0);
+    this.setInputRange(-360.0, 360.0);
+    this.enable();
+  }
+
+  public void turnAngle(double angle) {
+    angle = MathLib.constrain(angle, -360, 360);
+    setGyroPID();
+    setSetpoint(angle);
+  }
+
+  public void setMotorSpeeds(double left, double right) {
+    // positive setpoint to left side makes it go backwards
+    // positive setpoint to right side makes it go forwards.
+    frontLeft.set(-left);
+    rearLeft.set(-left);
+    frontRight.set(right);
+    rearRight.set(right);
+  }
+
+  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
+  private static double gp = 0.018, gi = 0.000015, gd = 0;
 }

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index a707777e96d7ac9bae06e4105f8a349323658957..a9e9ecd5148c0f76dd045c8e38b4550dc47373cf 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -49,7 +49,7 @@ public class IntakeArm extends Subsystem {
   }
 
   public void stopRollers() {
-    intakeRoller.set(Constants.IntakeArm.STOP_SPEED);
+    intakeRoller.set(0);
   }
 
   /***