From d8f30fce153007b50675ab4b78374ef54da6955a Mon Sep 17 00:00:00 2001 From: VisionDave Date: Sat, 10 Mar 2018 01:05:55 -0700 Subject: [PATCH] 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 | 2 + .../robot/commands/DriveStraightBasicAbs.java | 115 ++++++++++++++++++ .../robot/commands/auton/CenterLeftAbs.java | 46 +++++++ .../robot/commands/auton/CenterRightAbs.java | 45 +++++++ 4 files changed, 208 insertions(+) create mode 100644 src/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java create mode 100644 src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java create mode 100644 src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java diff --git a/src/org/usfirst/frc4388/robot/Constants.java b/src/org/usfirst/frc4388/robot/Constants.java index 71e8706..66adc89 100644 --- a/src/org/usfirst/frc4388/robot/Constants.java +++ b/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; diff --git a/src/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/src/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java new file mode 100644 index 0000000..21fede7 --- /dev/null +++ b/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(); + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java new file mode 100644 index 0000000..c4e3981 --- /dev/null +++ b/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)); + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java new file mode 100644 index 0000000..538a7db --- /dev/null +++ b/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)); + } +}