Auton base

org/bitbuckets/frc2014/RandomConstants.java

@@ -26,20 +26,25 @@ public class RandomConstants {
 
     /** The amount of ticks given out by the encoder per rotation. **/
     public static final int TICKS_PER_ROTATION = 1000;
-
     /** The amount of encoder ticks it takes to go one inch. Ticks per rotation divided by wheel circumference. **/
-    public static final int TICKS_PER_INCH = TICKS_PER_ROTATION/1;
+    public static final int TICKS_PER_INCH = (int)(TICKS_PER_ROTATION/(4*Math.PI));
     /** The KP for the pid controller used to drive n inches. **/
     public static final double INCH_PID_KP = 0.3;
     /** The KI for the pid controller used to drive n inches. **/
     public static final double INCH_PID_KI = 0.3;
     /** The KD for the pid controller used to drive n inches. **/
     public static final double INCH_PID_KD = 0.3;
-    /** The KF for the pid controller used to drive n inches. **/
-    public static final double INCH_PID_KF = 0.3;
-    /** The tolerence for the pid controller used to drive n inches. **/
-    public static final double INCH_PID_TOL = .1;
+    /** The max acceleration for the auton drivestraight. **/
+    public static final double INCH_MAX_ACCEL = .1;
 
+    /** The KP for the pid controller used to turn n degrees. **/
+    public static final double DEG_PID_KP = 0.3;
+    /** The KI for the pid controller used to turn n degrees.. **/
+    public static final double DEG_PID_KI = 0.3;
+    /** The KD for the pid controller used to turn n degrees. **/
+    public static final double DEG_PID_KD = 0.3;
+    /** The max acceleration for the auton driveturn. **/
+    public static final double DEG_MAX_ACCEL = .1;
 
     ///////////////----Catapault----///////////////
     /** The speed of the winch. **/

org/bitbuckets/frc2014/RobotMap.java

@@ -102,20 +102,28 @@ public class RobotMap {
     /**
      * The right encoder for the drivetrain.
      */
-    public static final int R_ENCODER_B = 2;
+    public static final int R_ENCODER_B = 3;
     /**
      * The left encoder for the drivetrain.
      */
     public static final int L_ENCODER_A = 4;
     /**
      * The left encoder for the drivetrain.
      */
-    public static final int L_ENCODER_B = 4;
+    public static final int L_ENCODER_B = 5;
     /**
      * The switch that tells the compressor the max pressure
      */
     public static final int PRESSURE_SWITCH = 14;
 
+    ////////////////////////////////////////////////////////////////////////////
+    /////////////////////////////////----AI----/////////////////////////////////
+    ////////////////////////////////////////////////////////////////////////////
+
+    /**
+     * The gyroscope.
+     */
+    public static final int GYRO = 1;
 
     ///////////////////////////////////////////////////////////////////////////////
     /////////////////////////////////----OTHER----/////////////////////////////////

org/bitbuckets/frc2014/commands/DriveStraight.java

@@ -0,0 +1,117 @@
+/* FRC 4183 - The Bit Buckets
+ * Tucson, AZ
+ *
+ * FRC 2014 Codebase
+ */
+package org.bitbuckets.frc2014.commands;
+
+import edu.wpi.first.wpilibj.command.PIDCommand;
+import org.bitbuckets.frc2014.RandomConstants;
+
+/**
+ *
+ * @author James Wyeth james.wyeth@gmail.com
+ */
+public class DriveStraight extends PIDCommand {
+    private int state;
+    private double distance;
+    private double maxVel;
+    private long timeInit;
+
+    /**
+     * 
+     * 
+     * @param dist
+     * @param vel 
+     */
+    public DriveStraight(int dist, double vel) {
+        super("DriveStriaght", RandomConstants.INCH_PID_KP, RandomConstants.INCH_PID_KI, RandomConstants.INCH_PID_KD);
+        distance = dist;
+        maxVel = vel;
+        setSetpoint(0);
+    }
+
+    /**
+     * 
+     */
+    protected void initialize() {
+        timeInit = System.currentTimeMillis();
+        state = 1;
+    }
+
+    /**
+     * 
+     */
+    protected void execute() {
+        switch(state){
+            case 1:
+                setSetpoint(RandomConstants.INCH_MAX_ACCEL*(System.currentTimeMillis()-timeInit)/1000);
+                if(getSetpoint() >= maxVel){
+                    state = 2;
+                }
+                if((CommandBase.driveTrain.getEncDistL() + CommandBase.driveTrain.getEncDistR()) >= distance){
+                    state = 3;
+                    maxVel = getSetpoint();
+                    timeInit = System.currentTimeMillis();
+                }
+                break;
+            case 2:
+                setSetpoint(maxVel);
+                //the distance under the last trapezoid of the curve
+                if(distance - (CommandBase.driveTrain.getEncDistL() + CommandBase.driveTrain.getEncDistR())/2 <= (maxVel*maxVel)/RandomConstants.INCH_MAX_ACCEL){
+                    state = 4;
+                    timeInit = System.currentTimeMillis();
+                }
+                break;
+            case 3:
+                setSetpoint(maxVel-RandomConstants.INCH_MAX_ACCEL*(System.currentTimeMillis()-timeInit)/1000);
+                if(getSetpoint() <= 0){
+                    state = 4;
+                }
+                break;
+            case 4:
+                setSetpoint(0);
+                state = 5;
+                break;
+        }
+    }
+
+    /**
+     * 
+     * 
+     * @return 
+     */
+    protected boolean isFinished() {
+        return state == 5;
+    }
+
+    /**
+     * 
+     */
+    protected void end() {
+    }
+
+    /**
+     * 
+     */
+    protected void interrupted() {
+    }
+
+    /**
+     * 
+     * 
+     * @return 
+     */
+    protected double returnPIDInput() {
+        return (CommandBase.driveTrain.getEncRateL() + CommandBase.driveTrain.getEncRateR())/2;
+    }
+
+    /**
+     * 
+     * 
+     * @param output 
+     */
+    protected void usePIDOutput(double output) {
+        CommandBase.driveTrain.tankDrive(output, output);
+    }
+}

org/bitbuckets/frc2014/commands/DriveTurn.java

@@ -0,0 +1,100 @@
+/* FRC 4183 - The Bit Buckets
+ * Tucson, AZ
+ *
+ * FRC 2014 Codebase
+ */
+package org.bitbuckets.frc2014.commands;
+
+import edu.wpi.first.wpilibj.command.PIDCommand;
+import org.bitbuckets.frc2014.RandomConstants;
+
+/**
+ *
+ * @author James Wyeth james.wyeth@gmail.com
+ */
+public class DriveTurn extends PIDCommand {
+    private int sign;
+    private int state;
+    private double angle;
+    private double maxVel;
+    private long timeInit;
+
+    public DriveTurn(double ang, double vel) {
+        super("DriveTurn", RandomConstants.DEG_PID_KP, RandomConstants.DEG_PID_KI, RandomConstants.DEG_PID_KD);
+        angle = ang;
+        maxVel = vel;
+        sign = ang > 0? 1 : -1;
+        setSetpoint(0);
+    }
+
+    /**
+     * 
+     */
+    protected void initialize() {
+        timeInit = System.currentTimeMillis();
+        state = 1;
+    }
+
+    /**
+     * 
+     */
+    protected void execute() {
+        switch(state){
+            case 1:
+                setSetpoint(RandomConstants.DEG_MAX_ACCEL*(System.currentTimeMillis()-timeInit)/1000);
+                if(getSetpoint() >= maxVel){
+                    state = 2;
+                }
+                if(CommandBase.driveTrain.getGyroAngle() >= angle/2){
+                    state = 3;
+                    maxVel = getSetpoint();
+                    timeInit = System.currentTimeMillis();
+                }
+                break;
+            case 2:
+                setSetpoint(maxVel);
+                //the distance under the last trapezoid of the curve
+                if(angle - CommandBase.driveTrain.getGyroAngle() <= (maxVel*maxVel)/RandomConstants.DEG_MAX_ACCEL){
+                    state = 4;
+                    timeInit = System.currentTimeMillis();
+                }
+                break;
+            case 3:
+                setSetpoint(maxVel-RandomConstants.DEG_MAX_ACCEL*(System.currentTimeMillis()-timeInit)/1000);
+                if(getSetpoint() <= 0){
+                    state = 4;
+                }
+                break;
+            case 4:
+                setSetpoint(0);
+                state = 5;
+                break;
+        }
+    }
+
+    /**
+     * 
+     * 
+     * @return 
+     */
+    protected boolean isFinished() {
+        return state == 5;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+
+    protected double returnPIDInput() {
+        return CommandBase.driveTrain.getGyroRate();
+    }
+
+    protected void usePIDOutput(double output) {
+        CommandBase.driveTrain.tankDrive(output*sign, -output*sign);
+    }
+}

org/bitbuckets/frc2014/commands/MotorDriveTime.java

@@ -0,0 +1,73 @@
+/* FRC 4183 - The Bit Buckets
+ * Tucson, AZ
+ *
+ * FRC 2014 Codebase
+ */
+package org.bitbuckets.frc2014.commands;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * Drives a specified speed controller for a certain time at a specified speed.
+ * 
+ * @author James Wyeth james.wyeth@gmail.com
+ */
+public class MotorDriveTime extends CommandBase {
+    /** The speed controller the motor to drive is connected to. **/
+    private SpeedController motor;
+    /** The time when the command was started. Used to check the time the motor has driven for. **/
+    private long timeInit;
+    /** The speed at which the motor is run. A double from 0 to 1. **/
+    private double speed;
+    /** The amount of time the motor is run in milliseconds. **/
+    private int time;
+
+    /**.
+     * The constructor. Sets the fields.
+     * 
+     * @param controller The speed controller the motor to drive is connected to.
+     * @param speedPercent The speed at which the motor is run. A double from 0 to 1.
+     * @param timeMillis The amount of time the motor should be run in milliseconds.
+     */
+    public MotorDriveTime(SpeedController controller, double speedPercent, int timeMillis) {
+        motor = controller;
+        speed = speedPercent;
+        time = timeMillis;
+    }
+
+    /**
+     * Initializes the command by sitting the initial time.
+     */
+    protected void initialize() {
+        timeInit = System.currentTimeMillis();
+    }
+
+    /**
+     * Called repeatedly. Set the speed of the motor.
+     */
+    protected void execute() {
+        motor.set(speed);
+    }
+
+    /**
+     * Tells the controller whether the command is finished.
+     * 
+     * @return Returns <code>true</code> if the command is finished. <code>false</code> otherwise.
+     */
+    protected boolean isFinished() {
+        return System.currentTimeMillis() - timeInit >= time;
+    }
+
+    /**
+     * The code to run after <code>isFinished</code> returns true. Stops the motor.
+     */
+    protected void end() {
+        motor.set(0);
+    }
+
+    /**
+     * The code to run if the command is interrupted. Calls <code>end()</code>.
+     */
+    protected void interrupted() {
+    }
+}