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

Implements Drive Straight

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-01 15:34:40 -07:00
parent 913f9b8a9e
commit e0966aa65b
2 changed files with 14 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+13 -6
View File
@@ -21,6 +21,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
boolean m_isInterrupted;
/**
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -54,6 +55,11 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
if (m_isInterrupted) {
resetGyroTarget();
m_isInterrupted = false;
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
@@ -78,14 +84,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double cosMultiplier = .7;
double steerOutput = 0;
double deadzone = .2;
double deadzone = .1;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
if (steer > 0) {
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
} else if (steer < 0) {
steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
@@ -108,6 +114,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_isInterrupted = interrupted;
}
// Returns true when the command should end.