diff --git a/org/bitbuckets/frc2014/RandomConstants.java b/org/bitbuckets/frc2014/RandomConstants.java
index dbbce2b..9378ce0 100644
--- a/org/bitbuckets/frc2014/RandomConstants.java
+++ b/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. **/
diff --git a/org/bitbuckets/frc2014/RobotMap.java b/org/bitbuckets/frc2014/RobotMap.java
index 336e6e0..675e551 100644
--- a/org/bitbuckets/frc2014/RobotMap.java
+++ b/org/bitbuckets/frc2014/RobotMap.java
@@ -102,7 +102,7 @@ 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.
*/
@@ -110,12 +110,20 @@ public class RobotMap {
/**
* 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----/////////////////////////////////
diff --git a/org/bitbuckets/frc2014/commands/DriveStraight.java b/org/bitbuckets/frc2014/commands/DriveStraight.java
new file mode 100644
index 0000000..e091b5b
--- /dev/null
+++ b/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);
+ }
+}
diff --git a/org/bitbuckets/frc2014/commands/DriveTurn.java b/org/bitbuckets/frc2014/commands/DriveTurn.java
new file mode 100644
index 0000000..bbfb698
--- /dev/null
+++ b/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);
+ }
+}
diff --git a/org/bitbuckets/frc2014/commands/MotorDriveTime.java b/org/bitbuckets/frc2014/commands/MotorDriveTime.java
new file mode 100644
index 0000000..65bdc22
--- /dev/null
+++ b/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 true if the command is finished. false otherwise.
+ */
+ protected boolean isFinished() {
+ return System.currentTimeMillis() - timeInit >= time;
+ }
+
+ /**
+ * The code to run after isFinished returns true. Stops the motor.
+ */
+ protected void end() {
+ motor.set(0);
+ }
+
+ /**
+ * The code to run if the command is interrupted. Calls end().
+ */
+ protected void interrupted() {
+ }
+}
diff --git a/org/bitbuckets/frc2014/commands/WaitMillis.java b/org/bitbuckets/frc2014/commands/WaitMillis.java
new file mode 100644
index 0000000..b21ce8e
--- /dev/null
+++ b/org/bitbuckets/frc2014/commands/WaitMillis.java
@@ -0,0 +1,60 @@
+/*
+ * To change this license header, choose License Headers in Project Properties.
+ * To change this template file, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.bitbuckets.frc2014.commands;
+
+/**
+ *
+ * @author james
+ */
+public class WaitMillis extends CommandBase {
+ /** The time at the beginning of the command. **/
+ private long timeInit;
+ /** The time to wait for. **/
+ private int time;
+
+ /**
+ * Constructor. Sets the time to wait for.
+ *
+ * @param timeMillis The time to wait.
+ */
+ public WaitMillis(int timeMillis) {
+ time = timeMillis;
+ }
+
+ /**
+ * Called at the beginning of the command.
+ */
+ protected void initialize() {
+ timeInit = System.currentTimeMillis();
+ }
+
+ /**
+ * Called continuously while the command is running. Does nothing.
+ */
+ protected void execute() {
+ }
+
+ /**
+ * Tells the command whether the tame has passed yet.
+ *
+ * @return Whether the time has passed.
+ */
+ protected boolean isFinished() {
+ return System.currentTimeMillis() - timeInit >= time;
+ }
+
+ /**
+ * Called when isFinished returns false.
+ */
+ protected void end() {
+ }
+
+ /**
+ * Called if something else needs the required subsystem. Never called in this command.
+ */
+ protected void interrupted() {
+ }
+}
diff --git a/org/bitbuckets/frc2014/subsystems/DriveTrain.java b/org/bitbuckets/frc2014/subsystems/DriveTrain.java
index 494e74a..799c6ea 100644
--- a/org/bitbuckets/frc2014/subsystems/DriveTrain.java
+++ b/org/bitbuckets/frc2014/subsystems/DriveTrain.java
@@ -6,6 +6,8 @@
package org.bitbuckets.frc2014.subsystems;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.bitbuckets.frc2014.RandomConstants;
@@ -18,12 +20,18 @@
* Collection of actuators and sensors that form the drive train subsystem.
*/
public class DriveTrain extends Subsystem {
- /** Base driving system standard in wpilibj */
- public RobotDrive drive;
/** Forward/backward power ranging from -1 (bwd) to 1 (fwd) */
double throttle = 0;
/** Rotational power ranging from -1 (ccw) to 1 (cw) */
double rotation = 0;
+ /** Base driving system standard in wpilibj **/
+ public RobotDrive drive;
+ /** Right encoder. **/
+ private Encoder encR;
+ /** Left encoder. **/
+ private Encoder encL;
+ /** Gyroscope. **/
+ private Gyro gyro;
/**
* Drivetrain constructor, sets up basic robot drive.
@@ -32,6 +40,12 @@ public DriveTrain() {
super();
drive = new RobotDrive(RobotMap.R_MOTOR_A, RobotMap.R_MOTOR_B, RobotMap.L_MOTOR_A, RobotMap.L_MOTOR_B);
drive.setExpiration(.25);
+ encR = new Encoder(RobotMap.R_ENCODER_A, RobotMap.R_ENCODER_B);
+ encR.start();
+ encL = new Encoder(RobotMap.L_ENCODER_A, RobotMap.L_ENCODER_B);
+ encL.start();
+ gyro = new Gyro(RobotMap.GYRO);
+ gyro.reset();
}
/**
@@ -41,6 +55,75 @@ public void initDefaultCommand() {
}
+ /**
+ * Gets the distance of the right encoder since the last reset.
+ *
+ * @return The distance of the right encoder.
+ */
+ public double getEncDistR(){
+ return encR.getDistance();
+ }
+
+ /**
+ * Gets the distance of the left encoder since the last reset.
+ *
+ * @return The distance of the left encoder.
+ */
+ public double getEncDistL(){
+ return encL.getDistance();
+ }
+
+ /**
+ * Gets the rate of the right encoder.
+ *
+ * @return The rate of the right encoder.
+ */
+ public double getEncRateR(){
+ return encR.getRate();
+ }
+
+ /**
+ * Gets the rate of the left encoder.
+ *
+ * @return The rate of the left encoder.
+ */
+ public double getEncRateL(){
+ return encL.getRate();
+ }
+
+ /**
+ * Resets the encoders to 0.
+ */
+ public void resetEncoders(){
+ encR.reset();
+ encL.reset();
+ }
+
+ /**
+ * Gets the angle of the gyro.
+ *
+ * @return The angle of the gyro.
+ */
+ public double getGyroAngle(){
+ return gyro.getAngle();
+ }
+
+ /**
+ * Gets the rate that the gyro is turning.
+ *
+ * @return
+ */
+ public double getGyroRate(){
+ return gyro.getRate();
+ }
+
+ /**
+ * Resets the gyro to an angle of 0.
+ */
+ public void resetGyro(){
+ gyro.reset();
+ }
+
/**
* Basic driving method. Can be used in autonomous and teleop.
*
@@ -53,6 +136,16 @@ public void drive(double throttle, double rotation){
drive.arcadeDrive(-throttle, -rotation);
}
+ /**
+ * Used for auton.
+ *
+ * @param left
+ * @param right
+ */
+ public void tankDrive(double left, double right){
+ drive.tankDrive(left, right);
+ }
+
/**
* More sophicsticated driving method for teleop based on team 254's
* "cheesy drive".
@@ -96,8 +189,7 @@ private double skim(double val) {
* @param maxChange The maximum amount the value can change per iteration.
* @return The limited value.
*/
- public double accelerationLimiter(double oldValue,
- double requestedValue, double maxChange) {
+ public double accelerationLimiter(double oldValue, double requestedValue, double maxChange) {
if(requestedValue - oldValue > maxChange) {
return oldValue + maxChange;
} else if(oldValue - requestedValue > maxChange) {