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

Fixed Drive With Joysticks

Browse Source
This commit is contained in:
Aarav Shah
2020-02-27 17:14:32 -07:00
parent 313ae0684c
commit 82286cb180
1 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+12 -10
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
import frc4388.utility.controller.IHandController;
@@ -42,22 +43,23 @@ public class DriveWithJoystick extends CommandBase {
double steerInput = -m_controller.getLeftYAxis();
double moveOutput = 0;
double steerOutput = 0;
if (moveInput >= 0){
moveOutput = -Math.cos(1.571*moveInput)+1;
if (steerInput >= 0){
steerOutput = -Math.cos(1.571*steerInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
steerOutput = Math.cos(1.571*steerInput)-1;
}
double cosMultiplier = .55;
double cosMultiplier = 1.0;
double deadzone = .1;
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;
if (moveInput > 0){
moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
} else if (moveInput < 0) {
moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
} else {
steerOutput = 0;
moveOutput = 0;
}
SmartDashboard.putNumber("Steer Output Test", moveOutput);
m_drive.driveWithInput(moveOutput, steerOutput);
}