From 7f1443a4ef74359b42e30fbcb9c97b1a8027c8f9 Mon Sep 17 00:00:00 2001 From: atomicdiamond Date: Fri, 18 Mar 2016 20:43:36 -0700 Subject: [PATCH] Fixes for Practice Bot -Added CDH + implementation -Let Spaghetti break gravity constant -Fixed launcher --- .../team3925/robot2016/CheesyDriveHelper.java | 147 ++++++++++++++++++ src/com/team3925/robot2016/Constants.java | 6 +- src/com/team3925/robot2016/OI.java | 25 +-- src/com/team3925/robot2016/Robot.java | 4 +- src/com/team3925/robot2016/RobotMap.java | 6 +- .../robot2016/commands/ManualDrive.java | 7 + .../robot2016/commands/PlanZAuto.java | 2 +- .../robot2016/subsystems/DriveTrain.java | 5 +- 8 files changed, 185 insertions(+), 17 deletions(-) create mode 100644 src/com/team3925/robot2016/CheesyDriveHelper.java diff --git a/src/com/team3925/robot2016/CheesyDriveHelper.java b/src/com/team3925/robot2016/CheesyDriveHelper.java new file mode 100644 index 0000000..67f5585 --- /dev/null +++ b/src/com/team3925/robot2016/CheesyDriveHelper.java @@ -0,0 +1,147 @@ +package com.team3925.robot2016; + +import com.team3925.robot2016.subsystems.DriveTrain; +import com.team3925.robot2016.util.DriveTrainSignal; +import com.team3925.robot2016.util.MiscUtil; + +import edu.wpi.first.wpilibj.DriverStation; + +/** + * CheesyDriveHelper implements the calculations used in CheesyDrive, sending + * power to the motors. + */ +public class CheesyDriveHelper { + + private DriveTrain drive; + double oldWheel, quickStopAccumulator; + private double throttleDeadband = 0.02; + private double wheelDeadband = 0.02; + private DriveTrainSignal signal = new DriveTrainSignal(0, 0); + + public CheesyDriveHelper(DriveTrain drive) { + this.drive = drive; + } + + public void cheesyDrive(double throttle, double wheel, boolean isQuickTurn, + boolean isHighGear) { + if (DriverStation.getInstance().isAutonomous()) { + return; + } + + double wheelNonLinearity; + + wheel = handleDeadband(wheel, wheelDeadband); + throttle = handleDeadband(throttle, throttleDeadband); + + double negInertia = wheel - oldWheel; + oldWheel = wheel; + + if (isHighGear) { + wheelNonLinearity = 0.6; + // Apply a sin function that's scaled to make it feel better. + wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) + / Math.sin(Math.PI / 2.0 * wheelNonLinearity); + wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) + / Math.sin(Math.PI / 2.0 * wheelNonLinearity); + } else { + wheelNonLinearity = 0.5; + // Apply a sin function that's scaled to make it feel better. + wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) + / Math.sin(Math.PI / 2.0 * wheelNonLinearity); + wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) + / Math.sin(Math.PI / 2.0 * wheelNonLinearity); + wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) + / Math.sin(Math.PI / 2.0 * wheelNonLinearity); + } + + double leftPwm, rightPwm, overPower; + double sensitivity; + + double angularPower; + double linearPower; + + // Negative inertia! + double negInertiaAccumulator = 0.0; + double negInertiaScalar; + if (isHighGear) { + negInertiaScalar = 4.0; + sensitivity = Constants.kDriveSensitivity; + } else { + if (wheel * negInertia > 0) { + negInertiaScalar = 2.5; + } else { + if (Math.abs(wheel) > 0.65) { + negInertiaScalar = 5.0; + } else { + negInertiaScalar = 3.0; + } + } + sensitivity = .85; // Constants.sensitivityLow.getDouble(); + } + double negInertiaPower = negInertia * negInertiaScalar; + negInertiaAccumulator += negInertiaPower; + + wheel = wheel + negInertiaAccumulator; + if (negInertiaAccumulator > 1) { + negInertiaAccumulator -= 1; + } else if (negInertiaAccumulator < -1) { + negInertiaAccumulator += 1; + } else { + negInertiaAccumulator = 0; + } + linearPower = throttle; + + // Quickturn! + if (isQuickTurn) { + if (Math.abs(linearPower) < 0.2) { + double alpha = 0.1; + quickStopAccumulator = (1 - alpha) * quickStopAccumulator + + alpha * MiscUtil.limit(wheel, 1.0) * 5; + } + overPower = 1.0; + if (isHighGear) { + sensitivity = 1.0; + } else { + sensitivity = 1.0; + } + angularPower = wheel; + } else { + overPower = 0.0; + angularPower = Math.abs(throttle) * wheel * sensitivity + - quickStopAccumulator; + if (quickStopAccumulator > 1) { + quickStopAccumulator -= 1; + } else if (quickStopAccumulator < -1) { + quickStopAccumulator += 1; + } else { + quickStopAccumulator = 0.0; + } + } + + rightPwm = leftPwm = linearPower; + leftPwm += angularPower; + rightPwm -= angularPower; + + if (leftPwm > 1.0) { + rightPwm -= overPower * (leftPwm - 1.0); + leftPwm = 1.0; + } else if (rightPwm > 1.0) { + leftPwm -= overPower * (rightPwm - 1.0); + rightPwm = 1.0; + } else if (leftPwm < -1.0) { + rightPwm += overPower * (-1.0 - leftPwm); + leftPwm = -1.0; + } else if (rightPwm < -1.0) { + leftPwm += overPower * (-1.0 - rightPwm); + rightPwm = -1.0; + } + signal.left = leftPwm; + signal.right = rightPwm; + drive.setMotorSpeeds(signal); + } + + public double handleDeadband(double val, double deadband) { + return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0; + } + +} diff --git a/src/com/team3925/robot2016/Constants.java b/src/com/team3925/robot2016/Constants.java index f8d1d4e..9b23aee 100644 --- a/src/com/team3925/robot2016/Constants.java +++ b/src/com/team3925/robot2016/Constants.java @@ -11,7 +11,7 @@ public class Constants { // Loop time of the program public static final double DELTA_TIME = 0.020; // 20 ms TODO Will be bigger due to GRIP - public static final boolean DO_LOG_AHRS_VALUES = true;//Robot.prefs.getBoolean("Do Log AHRS Vals", false); + public static final boolean DO_LOG_AHRS_VALUES = false;//Robot.prefs.getBoolean("Do Log AHRS Vals", false); public static final boolean DO_LOG_PDP_VALUES = false;//Robot.prefs.getBoolean("Do Log PDP Vals", false); public static final boolean DO_LOG_GRIP_VALUES = false;//Robot.prefs.getBoolean("Do Log GRIP Vals", false); @@ -92,7 +92,7 @@ public static final void initLauncherIntakeTable() { // DRIVETRAIN CONSTANTS - + public static double kDriveSensitivity = 0.75; public static final double GLOBAL_MAX_DRIVE_TRAIN_PWR = 0.8;//Robot.prefs.getDouble("Max DriveTrain Pwr", 1); public static final double DRIVE_TRAIN_VOLTAGE_RAMP_RATE = 1; @@ -252,4 +252,6 @@ public static final void initLauncherIntakeTable() { + + } diff --git a/src/com/team3925/robot2016/OI.java b/src/com/team3925/robot2016/OI.java index 1268308..0ed7638 100644 --- a/src/com/team3925/robot2016/OI.java +++ b/src/com/team3925/robot2016/OI.java @@ -1,7 +1,5 @@ package com.team3925.robot2016; -import static com.team3925.robot2016.util.XboxHelper.AXIS_LEFT_Y; -import static com.team3925.robot2016.util.XboxHelper.AXIS_RIGHT_X; import static com.team3925.robot2016.util.XboxHelper.START; import java.text.DecimalFormat; @@ -19,7 +17,8 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -56,6 +55,7 @@ public final class OI { public Joystick xboxDriver; public Joystick xboxShooter; + public Joystick wheel; public Button startCollectBall; public Button startThrowBallFar; @@ -80,6 +80,7 @@ public OI() { xboxDriver = new Joystick(0); xboxShooter = new Joystick(1); + wheel = new Joystick(2); collectBall = new CollectBall(); throwBallFar = new ThrowBall(65, 1, 5); @@ -175,15 +176,22 @@ private void addThrowBallValue(double percentage) { // ROBOT BEHAVIOR public double getManualDrive_ForwardValue() { - return XboxHelper.getDriverAxis(AXIS_LEFT_Y); +// return XboxHelper.getDriverAxis(AXIS_LEFT_Y); + return xboxDriver.getRawAxis(1); } public double getManualDrive_RotateValue() { - return XboxHelper.getDriverAxis(AXIS_RIGHT_X); +// return -XboxHelper.getDriverAxis(AXIS_RIGHT_X); + return -wheel.getRawAxis(0); } public boolean getManualDrive_HighGearToggle() { - return XboxHelper.getDriverButton(XboxHelper.TRIGGER_LT) || XboxHelper.getDriverButton(XboxHelper.TRIGGER_RT); + return XboxHelper.getDriverButton(XboxHelper.TRIGGER_LT); + } + + public boolean getManualDrive_QuickTurn() { +// return XboxHelper.getDriverButton(XboxHelper.TRIGGER_RT); + return wheel.getRawButton(6) || wheel.getRawButton(5); } public boolean getStartCandyCanes() { @@ -207,7 +215,7 @@ public double getCandyCanes_Set() { } public boolean getManualArms_GetArmValue() { - return XboxHelper.getDriverAxis(XboxHelper.AXIS_TRIGGER_LEFT)>0.5 || XboxHelper.getDriverAxis(XboxHelper.AXIS_TRIGGER_RIGHT)>0.5; + return xboxDriver.getRawButton(1); } public boolean getCommandCancel() { @@ -239,5 +247,4 @@ public CommandGroup setAutonomous() { } -} - +} \ No newline at end of file diff --git a/src/com/team3925/robot2016/Robot.java b/src/com/team3925/robot2016/Robot.java index 54adc21..d3edcaa 100644 --- a/src/com/team3925/robot2016/Robot.java +++ b/src/com/team3925/robot2016/Robot.java @@ -52,6 +52,7 @@ public class Robot extends IterativeRobot implements SmartdashBoardLoggable { public static AHRS navx = null; public static PowerDistributionPanel pdp; public static OI oi; + public static CheesyDriveHelper cdh; //Commands CommandGroup autoRoutine; @@ -107,6 +108,7 @@ public void robotInit() { // pointers. Bad news. Don't move it. oi = new OI(); XboxHelper.init(); + cdh = new CheesyDriveHelper(driveTrain); //Creating Commands manualArms = new ManualPlexiArms(); @@ -193,7 +195,7 @@ public void teleopInit() { reset(); // driveTrain.setPIDEnabled(false); candyCanes.startTimeOut(); - manualCandyCanes.start(); +// manualCandyCanes.start(); // visionTest.start(); manualArms.start(); diff --git a/src/com/team3925/robot2016/RobotMap.java b/src/com/team3925/robot2016/RobotMap.java index d93442b..7a7aed3 100644 --- a/src/com/team3925/robot2016/RobotMap.java +++ b/src/com/team3925/robot2016/RobotMap.java @@ -218,6 +218,7 @@ public static void init() { LiveWindow.addActuator("Launcher", "MotorLeft", launcherMotorLeft); launcherMotorLeft.setFeedbackDevice(FeedbackDevice.QuadEncoder); launcherMotorLeft.changeControlMode(TalonControlMode.PercentVbus); + launcherMotorLeft.setInverted(false); launcherMotorLeft.reverseOutput(false); launcherMotorLeft.reverseSensor(true); // launcherMotorLeft.configEncoderCodesPerRev(4096); @@ -226,8 +227,9 @@ public static void init() { LiveWindow.addActuator("Launcher", "MotorRight", launcherMotorRight); launcherMotorRight.setFeedbackDevice(FeedbackDevice.QuadEncoder); launcherMotorRight.changeControlMode(TalonControlMode.PercentVbus); - launcherMotorLeft.reverseOutput(false); - launcherMotorRight.reverseSensor(false); + launcherMotorRight.setInverted(true); // practice = true | comp = false + launcherMotorRight.reverseOutput(true); + launcherMotorRight.reverseSensor(true); // launcherMotorRight.configEncoderCodesPerRev(4096); //END LAUNCHER diff --git a/src/com/team3925/robot2016/commands/ManualDrive.java b/src/com/team3925/robot2016/commands/ManualDrive.java index 61d997e..5c6eaf7 100644 --- a/src/com/team3925/robot2016/commands/ManualDrive.java +++ b/src/com/team3925/robot2016/commands/ManualDrive.java @@ -8,6 +8,7 @@ import com.team3925.robot2016.util.SmartdashBoardLoggable; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * @@ -30,6 +31,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { + /* // negate fwd on practice robot fwdSet = oi.getManualDrive_ForwardValue(); turnSet = oi.getManualDrive_RotateValue(); @@ -53,6 +55,9 @@ protected void execute() { lastFwdSet = fwdSet; lastTurnSet = turnSet; shiftWasPressed = shiftPressed; + */ + + Robot.cdh.cheesyDrive(oi.getManualDrive_ForwardValue(), oi.getManualDrive_RotateValue(), oi.getManualDrive_QuickTurn(), driveTrain.isHighGear()); logData(); } @@ -77,6 +82,8 @@ protected void interrupted() { public void logData() { putNumberSD("FwdPwr", fwdSet); putNumberSD("TurnPwr", turnSet); + Constants.kDriveSensitivity = SmartDashboard.getNumber(getFormattedName() + "Sensitivity", Constants.kDriveSensitivity); + putNumberSD("Sensitivity", Constants.kDriveSensitivity); } @Override diff --git a/src/com/team3925/robot2016/commands/PlanZAuto.java b/src/com/team3925/robot2016/commands/PlanZAuto.java index 5d7e41d..bd83952 100644 --- a/src/com/team3925/robot2016/commands/PlanZAuto.java +++ b/src/com/team3925/robot2016/commands/PlanZAuto.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.command.Command; -public class PlanZAuto extends Command{ +public class PlanZAuto extends Command { PlexiArms plexi = new PlexiArms(); DriveTrain drive = new DriveTrain(); diff --git a/src/com/team3925/robot2016/subsystems/DriveTrain.java b/src/com/team3925/robot2016/subsystems/DriveTrain.java index 30593cb..e2c9510 100644 --- a/src/com/team3925/robot2016/subsystems/DriveTrain.java +++ b/src/com/team3925/robot2016/subsystems/DriveTrain.java @@ -61,7 +61,7 @@ public void setMotorSpeeds(DriveTrainSignal input) { // } public void setHighGear(boolean highGear) { - shifterSolenoid.set(highGear ? Value.kReverse : Value.kForward); +// shifterSolenoid.set(highGear ? Value.kReverse : Value.kForward); } public void resetEncoders() { @@ -70,7 +70,8 @@ public void resetEncoders() { } public boolean isHighGear() { - return shifterSolenoid.get() == Value.kReverse; +// return shifterSolenoid.get() == Value.kReverse; + return false; } // public void setPIDEnabled(boolean enabled) {