Permalink
Browse files

CheesyDrive (Untested)

  • Loading branch information...
Maxcr1 committed Jul 17, 2018
1 parent 09bc7ed commit d621bd573d96be00f870b0fa4e03c70a58669dc1
@@ -231,4 +231,8 @@ public static void getWantedHookRelease() {
}
}

public static boolean getQuickTurn() {
return primaryLeft.getRawButton(6) || primaryRight.getRawButton(11);
}

}
@@ -41,13 +41,14 @@

}

public static boolean cheesyDrive = false;
public static DriveMode driveMode;
public static boolean ptoActive;

// DriveOverride Handler
public enum DriveType {

OVERRIDE_DRIVING, NORMAL_DRIVING, EXTERNAL_DRIVING, HANG
OVERRIDE_DRIVING, NORMAL_DRIVING, EXTERNAL_DRIVING, HANG, CHEESY_DRIVE

}

@@ -31,24 +31,8 @@ private static void setWantedMacro(Macro pos) {
// Wanted Driver Override Handler
private static void getWantedDriveOverride() {

boolean hangActive = true;

if(ControlMap.driversWantToHang()) {
RobotWantedStates.wantedDriveType = DriveType.HANG;
}
else if(ControlMap.driversWantToDrive()) {
hangActive = false;
}

if(!hangActive) {
if (ControlMap.primaryDriverOverride()) {
RobotWantedStates.wantedDriveType = DriveType.OVERRIDE_DRIVING;
}
// Drivers Want to Drive
else {
RobotWantedStates.wantedDriveType = DriveType.NORMAL_DRIVING;
}
}
}

// Wanted Driver Shift Handler
@@ -8,6 +8,8 @@
import com.team2169.robot.RobotStates.DriveMode;
import com.team2169.robot.RobotStates.DriveType;
import com.team2169.util.DebugPrinter;
import com.team2169.util.DriveManager;
import com.team2169.util.DriveSignal;
import com.team2169.util.FlyByWireHandler;

import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -38,10 +40,12 @@ public static DriveTrain getInstance() {
private DoubleSolenoid ptoShift;
public AHRS navX;
private NetworkTable table;
private DriveManager dManager;

private DriveTrain() {

table = NetworkTable.getTable("SmartDashboard");
dManager = new DriveManager();

//Fastest Robot will turn
table.putValue("Max Speed", Constants.turnMaxSpeed);
@@ -106,6 +110,12 @@ private DriveTrain() {

}

void cheesyDrive(DriveSignal signal) {
left.set(ControlMode.PercentOutput, signal.getLeft());
right.set(ControlMode.PercentOutput, signal.getRight());

}

void driveHandler() {

Constants.turnMaxError = table.getDouble("Max Error", 0);
@@ -117,16 +127,25 @@ void driveHandler() {

switch (RobotWantedStates.wantedDriveType) {


// Drive without Override
case NORMAL_DRIVING:
default:

shift();

// Drive with Acceleration Handler
left.set(ControlMode.PercentOutput, ControlMap.leftTankStick(false) * FlyByWireHandler.getSafeSpeed());
right.set(ControlMode.PercentOutput, ControlMap.rightTankStick(false) * FlyByWireHandler.getSafeSpeed());

if(RobotStates.cheesyDrive) {
cheesyDrive(dManager.cheesyDrive(ControlMap.leftTankStick(false), ControlMap.rightTankStick(false), ControlMap.getQuickTurn(),
RobotStates.driveMode != RobotStates.DriveMode.LOW));

break;
}
else {
left.set(ControlMode.PercentOutput, ControlMap.leftTankStick(false) * FlyByWireHandler.getSafeSpeed());
right.set(ControlMode.PercentOutput, ControlMap.rightTankStick(false) * FlyByWireHandler.getSafeSpeed());
}

RobotStates.driveTrainOverride = false;
RobotStates.driveType = DriveType.NORMAL_DRIVING;

@@ -137,10 +156,18 @@ void driveHandler() {

shift();

if(RobotStates.cheesyDrive) {
cheesyDrive(dManager.cheesyDrive(ControlMap.leftTankStick(false), ControlMap.rightTankStick(false), ControlMap.getQuickTurn(),
RobotStates.driveMode != RobotStates.DriveMode.LOW));

break;
}
else {
// Drive with Override
left.set(ControlMode.PercentOutput, ControlMap.leftTankStick(false));
right.set(ControlMode.PercentOutput, ControlMap.rightTankStick(false));

left.set(ControlMode.PercentOutput, ControlMap.leftTankStick(false));
right.set(ControlMode.PercentOutput, ControlMap.rightTankStick(false));
}

RobotStates.driveTrainOverride = true;
RobotStates.driveType = DriveType.OVERRIDE_DRIVING;

@@ -68,7 +68,8 @@ public void startCompressor() {
comp.start();
}

public void subsystemLooper() {
@SuppressWarnings("deprecation")
public void subsystemLooper() {

DataSelector.looper();

@@ -0,0 +1,143 @@
package com.team2169.util;

/**
* Helper class to implement "Cheesy Drive". "Cheesy Drive" simply means that the "turning" stick controls the curvature
* of the robot's path rather than its rate of heading change. This helps make the robot more controllable at high
* speeds. Also handles the robot's quick turn functionality - "quick turn" overrides constant-curvature turning for
* turn-in-place maneuvers.
*/
public class DriveManager {

private static final double kThrottleDeadband = 0.02;
private static final double kWheelDeadband = 0.02;

// These factor determine how fast the wheel traverses the "non linear" sine curve.
private static final double kHighWheelNonLinearity = 0.65;
private static final double kLowWheelNonLinearity = 0.5;

private static final double kHighNegInertiaScalar = 4.0;

private static final double kLowNegInertiaThreshold = 0.65;
private static final double kLowNegInertiaTurnScalar = 3.5;
private static final double kLowNegInertiaCloseScalar = 4.0;
private static final double kLowNegInertiaFarScalar = 5.0;

private static final double kHighSensitivity = 0.95;
private static final double kLowSensitiity = 1.3;

private static final double kQuickStopDeadband = 0.2;
private static final double kQuickStopWeight = 0.1;
private static final double kQuickStopScalar = 5.0;

private double mOldWheel = 0.0;
private double mQuickStopAccumlator = 0.0;
private double mNegInertiaAccumlator = 0.0;

public DriveSignal cheesyDrive(double throttle, double wheel, boolean isQuickTurn,
boolean isHighGear) {

wheel = handleDeadband(wheel, kWheelDeadband);
throttle = handleDeadband(throttle, kThrottleDeadband);

double negInertia = wheel - mOldWheel;
mOldWheel = wheel;

double wheelNonLinearity;
if (isHighGear) {
wheelNonLinearity = kHighWheelNonLinearity;
final double denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity);
// Apply a sin function that's scaled to make it feel better.
wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator;
wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator;
} else {
wheelNonLinearity = kLowWheelNonLinearity;
final double denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity);
// Apply a sin function that's scaled to make it feel better.
wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator;
wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator;
wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator;
}

double leftPwm, rightPwm, overPower;
double sensitivity;

double angularPower;
double linearPower;

// Negative inertia!
double negInertiaScalar;
if (isHighGear) {
negInertiaScalar = kHighNegInertiaScalar;
sensitivity = kHighSensitivity;
} else {
if (wheel * negInertia > 0) {
// If we are moving away from 0.0, aka, trying to get more wheel.
negInertiaScalar = kLowNegInertiaTurnScalar;
} else {
// Otherwise, we are attempting to go back to 0.0.
if (Math.abs(wheel) > kLowNegInertiaThreshold) {
negInertiaScalar = kLowNegInertiaFarScalar;
} else {
negInertiaScalar = kLowNegInertiaCloseScalar;
}
}
sensitivity = kLowSensitiity;
}
double negInertiaPower = negInertia * negInertiaScalar;
mNegInertiaAccumlator += negInertiaPower;

wheel = wheel + mNegInertiaAccumlator;
if (mNegInertiaAccumlator > 1) {
mNegInertiaAccumlator -= 1;
} else if (mNegInertiaAccumlator < -1) {
mNegInertiaAccumlator += 1;
} else {
mNegInertiaAccumlator = 0;
}
linearPower = throttle;

// Quickturn!
if (isQuickTurn) {
if (Math.abs(linearPower) < kQuickStopDeadband) {
double alpha = kQuickStopWeight;
mQuickStopAccumlator = (1 - alpha) * mQuickStopAccumlator
+ alpha * Math.min(1, Math.max(-1, wheel)) * kQuickStopScalar;
}
overPower = 1.0;
angularPower = wheel;
} else {
overPower = 0.0;
angularPower = Math.abs(throttle) * wheel * sensitivity - mQuickStopAccumlator;
if (mQuickStopAccumlator > 1) {
mQuickStopAccumlator -= 1;
} else if (mQuickStopAccumlator < -1) {
mQuickStopAccumlator += 1;
} else {
mQuickStopAccumlator = 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;
}
return new DriveSignal(leftPwm, rightPwm);
}

public double handleDeadband(double val, double deadband) {
return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0;
}
}
@@ -0,0 +1,40 @@
package com.team2169.util;

/**
* A drivetrain command consisting of the left, right motor settings and whether the brake mode is enabled.
*/
public class DriveSignal {
protected double mLeftMotor;
protected double mRightMotor;
protected boolean mBrakeMode;

public DriveSignal(double left, double right) {
this(left, right, false);
}

public DriveSignal(double left, double right, boolean brakeMode) {
mLeftMotor = left;
mRightMotor = right;
mBrakeMode = brakeMode;
}

public static DriveSignal NEUTRAL = new DriveSignal(0, 0);
public static DriveSignal BRAKE = new DriveSignal(0, 0, true);

public double getLeft() {
return mLeftMotor;
}

public double getRight() {
return mRightMotor;
}

public boolean getBrakeMode() {
return mBrakeMode;
}

@Override
public String toString() {
return "L: " + mLeftMotor + ", R: " + mRightMotor + (mBrakeMode ? ", BRAKE" : "");
}
}
@@ -1,12 +1,20 @@
package com.team2169.util;

import com.team2169.robot.RobotStates;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class ShuffleBoardManager {

SendableChooser<Boolean> driveTypeChooser = new SendableChooser<>();

public void init() {

driveTypeChooser.addDefault("No", false);
driveTypeChooser.addObject("Yes", true);
SmartDashboard.putData(driveTypeChooser);
connected();

}
@@ -20,6 +28,7 @@ public void auto() {

public void teleOp() {

RobotStates.cheesyDrive = driveTypeChooser.getSelected();
connected();
SmartDashboard.putNumber("Match Time", DriverStation.getInstance().getMatchTime());

0 comments on commit d621bd5

Please sign in to comment.