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

integrated xboxraw

Browse Source
This commit is contained in:
Ryan Manley
2022-01-15 15:52:38 -07:00
parent 45574a2a06
commit 3061563db3
8 changed files with 93 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+33 -31
View File
@@ -97,37 +97,39 @@ public class SwerveDrive extends SubsystemBase {
*/
public void driveWithInput(double[] speeds, double rot, boolean fieldRelative)
{
if (speeds[0] == 0 && speeds[1] == 0) ignoreAngles = true;
if (speeds[0] == 0 && speeds[1] == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
// Temporary janky raw joysticks
float[] driveController = new float[HAL.kMaxJoystickAxes];
HAL.getJoystickAxes((byte) 0, driveController);
// ByteBuffer buttons = new
ByteBuffer num_buttons_buffer = ByteBuffer.allocateDirect(1);
int buttons_int = HAL.getJoystickButtons((byte) 0, num_buttons_buffer);
int num_buttons = num_buttons_buffer.get(0);
boolean[] buttons = new boolean[num_buttons];
for (int i = 0; i < num_buttons; i++) {
buttons[i] = (buttons_int & 1 << i) > 0;
}
if (buttons[0])
m_gyro.reset();
float leftXAxis = driveController[0];
float leftYAxis = driveController[1];
float rightXAxis = driveController[4];
leftXAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftXAxis;
leftYAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftYAxis;
rightXAxis = XboxController.inDeadZone(rightXAxis) ? 0.f : rightXAxis;
leftXAxis *= leftXAxis * leftXAxis;
leftYAxis *= leftYAxis * leftYAxis;
rightXAxis *= rightXAxis * rightXAxis;
double[] dashboardNums = new double[HAL.kMaxJoystickAxes];
for (int i = 0; i < HAL.kMaxJoystickAxes; i++) {
dashboardNums[i] = (double)((int)(driveController[i] * 100.f)) / 100.d;
}
SmartDashboard.putNumberArray("axes", dashboardNums);
speeds = XboxController.ClampJoystickAxis(leftXAxis, leftYAxis);
rot = -rightXAxis;
speeds[0] *= speeds[0] * speeds[0];
speeds[1] *= speeds[1] * speeds[1];
// // Temporary janky raw joysticks
// float[] driveController = new float[HAL.kMaxJoystickAxes];
// HAL.getJoystickAxes((byte) 0, driveController);
// // ByteBuffer buttons = new
// ByteBuffer num_buttons_buffer = ByteBuffer.allocateDirect(1);
// int buttons_int = HAL.getJoystickButtons((byte) 0, num_buttons_buffer);
// int num_buttons = num_buttons_buffer.get(0);
// boolean[] buttons = new boolean[num_buttons];
// for (int i = 0; i < num_buttons; i++) {
// buttons[i] = (buttons_int & 1 << i) > 0;
// }
// if (buttons[0])
// m_gyro.reset();
// float leftXAxis = driveController[0];
// float leftYAxis = driveController[1];
// float rightXAxis = driveController[4];
// leftXAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftXAxis;
// leftYAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftYAxis;
// rightXAxis = XboxController.inDeadZone(rightXAxis) ? 0.f : rightXAxis;
// leftXAxis *= leftXAxis * leftXAxis;
// leftYAxis *= leftYAxis * leftYAxis;
// rightXAxis *= rightXAxis * rightXAxis;
// double[] dashboardNums = new double[HAL.kMaxJoystickAxes];
// for (int i = 0; i < HAL.kMaxJoystickAxes; i++) {
// dashboardNums[i] = (double)((int)(driveController[i] * 100.f)) / 100.d;
// }
// SmartDashboard.putNumberArray("axes", dashboardNums);
// speeds = XboxController.ClampJoystickAxis(leftXAxis, leftYAxis);
// rot = -rightXAxis;
// System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]);
@@ -144,7 +146,7 @@ public class SwerveDrive extends SubsystemBase {
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state, ignoreAngle);
module.setDesiredState(state, ignoreAngles);
}
}