Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fix mode switching

Browse Source 
- Compact joystick drive math
This commit is contained in:
aarav18
2021-10-28 17:59:34 -06:00
parent 695ddfca77
commit 1503c7f027
2 changed files with 13 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -2
View File
@@ -38,7 +38,7 @@ public final class Constants {
int i = mode.ordinal() + 1;
Mode[] values = values();
i = i >= values.length ? 0 : i;
mode = values[i];
set(values[i]);
System.out.println(mode);
}
}
@@ -83,7 +83,7 @@ public final class Constants {
public static final int DRIVE_CRUISE_VELOCITY = 30000;
public static final int DRIVE_ACCELERATION = 23000;
private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.5 };
private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.8 };
public static double DRIVE_WITH_JOYSTICK_FACTOR;
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
@@ -264,4 +264,8 @@ public final class Constants {
public static final int XBOX_OPERATOR_ID = 1;
public static final int BUTTON_FOX_ID = 2;
}
public static final class MathConstants {
public static final double PI_2 = Math.PI / 2;
}
}
src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
+7 -27
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants;
import frc4388.robot.Robot;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.MathConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.utility.controller.IHandController;
@@ -44,34 +45,13 @@ public class DriveWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveInput = m_controller.getLeftYAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
double steerInput = m_controller.getRightXAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
double moveOutput = 0;
double steerOutput = 0;
if (moveInput >= 0){
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
moveOutput *= DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
System.out.println(moveOutput);
double cosMultiplier;
double deadzone = .1;
if (m_pneumatics.m_isSpeedShiftHigh) {
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
} else {
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
}
if (steerInput > 0){
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
} else if (steerInput < 0) {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
} else {
steerOutput = 0;
}
double moveOutput = (moveInput >= 0 ? -1 : 1) * (Math.cos(MathConstants.PI_2 * moveInput) - 1);
double cosMultiplier = m_pneumatics.m_isSpeedShiftHigh ? DriveConstants.COS_MULTIPLIER_HIGH : DriveConstants.COS_MULTIPLIER_LOW;
double deadzone = 0.1;
double steerOutput = (steerInput == 0 ? 0 : (steerInput > 0 ? -1 : 1)) * (Math.cos(MathConstants.PI_2 * steerInput) * (cosMultiplier - deadzone) + cosMultiplier);
/*
double outputLimit = 0.8;