Add DriveStraightBasicAbs

a variant of DSB that does NOT reset the gyro each time, so that absolute angles can be used

src/org/usfirst/frc4388/robot/Constants.java

@@ -21,6 +21,8 @@ public class Constants {
     public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0;
     public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0;
     public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0;
+    public static double kDriveStraightBasicYawErrorDivisor = 20.0;		// steer parameter will be -yawError divided by this
+    public static double kDriveStraightBasicMaxSteerMagnitude = 0.8;	// keep absolute value of steer parameter below this
     public static double kDriveTurnBasicTankMotorOutput = 0.5;
     public static double kDriveTurnBasicSingleMotorOutput = 0.15;
     public static double kElevatorWheelDiameterInches = 1;

src/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java

@@ -0,0 +1,115 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+public class DriveStraightBasicAbs extends Command {
+    private double m_targetInches;
+    private double m_maxVelocityInchesPerSec;
+    private double m_speed;
+    private boolean m_goingBackwards;
+    private boolean m_useGyroLock;
+    private boolean m_useAbsolute;
+    private double m_desiredAbsoluteAngle;
+	private double m_targetGyroAngleDeg;	// what we want the gyro to read as we're driving straight
+    private double m_commandInitTimestamp;
+	private double m_lastCommandExecuteTimestamp = 0.0;
+	private double m_lastCommandExecutePeriod = 0.0;
+ 
+    public DriveStraightBasicAbs(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
+        requires(Robot.drive);
+        m_targetInches = targetInches;
+        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
+        m_useGyroLock = useGyroLock;
+        m_useAbsolute = useAbsolute;
+        m_desiredAbsoluteAngle = desiredAbsoluteAngle;
+        m_goingBackwards = (m_targetInches < 0.0);
+   }
+    
+    protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) {
+    	double sign = (backwards ? -1.0 : 1.0);
+    	// Keep velocity above stiction limit (else bot will freeze and command will not complete)
+    	double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec);
+        // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick)
+    	return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec;
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+		double currentAngleDeg = Robot.drive.getGyroAngleDeg();
+		if (m_useAbsolute) {
+			m_targetGyroAngleDeg = m_desiredAbsoluteAngle;
+		} else {
+			m_targetGyroAngleDeg = currentAngleDeg;
+		}
+    	Robot.drive.resetEncoders();
+    	Robot.drive.setControlMode(DriveControlMode.RAW);
+    	// start out at half speed, as crude way to reduce slippage
+        m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards);
+		m_commandInitTimestamp = Timer.getFPGATimestamp();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+		// Measure *actual* update period
+		double currentTimestamp = Timer.getFPGATimestamp();
+		if (m_lastCommandExecuteTimestamp > 0.001)	{	// ie, if this is NOT the first time
+			m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
+		}
+		m_lastCommandExecuteTimestamp = currentTimestamp;
+    	double steer = 0.0;
+    	if (m_useGyroLock) {
+    		double yawError = Robot.drive.getGyroAngleDeg() - m_targetGyroAngleDeg;
+    		steer = -yawError / Constants.kDriveStraightBasicYawErrorDivisor;
+    		if (steer < -Constants.kDriveStraightBasicMaxSteerMagnitude) {
+    			steer = -Constants.kDriveStraightBasicMaxSteerMagnitude;
+    		} else if (Constants.kDriveStraightBasicMaxSteerMagnitude < steer) {
+    			steer = Constants.kDriveStraightBasicMaxSteerMagnitude;
+    		}
+    	}
+    	Robot.drive.rawMoveSteer(m_speed, steer);
+		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	boolean finished=false;
+    	double velocity = m_maxVelocityInchesPerSec;
+    	double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2;
+    	double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0);
+    	if (remaining < 0.0) {
+    		finished = true;
+    	} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
+    		velocity = m_maxVelocityInchesPerSec / 4.0;		// quarter speed
+    	} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) {		// last 300 ms
+    		velocity = m_maxVelocityInchesPerSec / 2.0;		// half speed
+    	}
+    	if (!finished) {
+    		m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
+    		SmartDashboard.putNumber("DSB Dist", position);
+    	} else {
+    		SmartDashboard.putNumber("DSB finDist", position);
+    	}
+		return finished;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+		double currentTimestamp = Timer.getFPGATimestamp();
+    	SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
+    	Robot.drive.rawMoveSteer(0.0, 0.0);
+		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java

@@ -0,0 +1,46 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class CenterLeftAbs extends CommandGroup {
+    
+    public CenterLeftAbs() 
+    {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
+    	addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasicAbs(53, 50, true, true, 130));
+    	addSequential(new ElevatorBasic(20));
+    	addSequential(new DriveTurnBasic(true, 164.5, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasicAbs(19, 50, true, true, 164.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasicAbs(-20, 50, true, true, 164.5));
+      	addSequential(new ElevatorBasic(10));
+    	addSequential(new DriveSpeedShift(false));
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java

@@ -0,0 +1,45 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class CenterRightAbs extends CommandGroup {
+    
+    public CenterRightAbs()
+    {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
+    	addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasicAbs(53, 50, true, true, -130));
+    	addSequential(new ElevatorBasic(20));
+    	addSequential(new DriveTurnBasic(true, -164.5, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasicAbs(19, 50, true, true, -164.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 50, true, true, -164.5));
+    	addSequential(new DriveSpeedShift(false));
+    }
+}