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

Account for High gear being a nice boy

Browse Source
This commit is contained in:
Aarav Shah
2020-02-21 22:03:41 -07:00
parent 5f989fb21a
commit 5015432107
3 changed files with 121 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+32 -12
View File
@@ -24,6 +24,22 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
IHandController m_controller;
boolean m_isInterrupted;
/* Deadassist Constants */
final float stopPosVelCoefLow = 1;
final float stopPosVelCoefHigh = 3;
final float cosMultiplierLow = 0.55f;
final float cosMultiplierHigh = 0.35f;
final float targetAngleCoefLow = 5;
final float targetAngleCoefHigh = 5;
final float gyroVelCoefLow = 1;
final float gyroVelCoefHigh = 3;
/* Deadassist Coeficients */
final float stopPosVelCoef = 1;
final float cosMultiplier = 0.55f;
final float targetAngleCoef = 5;
final float gyroVelCoef = 1;
/**
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
@@ -73,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
m_deadTimeSteer = m_currTime;
}
/* If move stick has been pressed within 1 sec */
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
if (m_drive.isSpeedShiftHigh) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If move stick has been pressed within 1 sec */
else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
@@ -131,7 +151,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
updateGyroTarget(steerOutput);
double currentPos = m_drive.m_rightFrontMotorPos;
if (Math.abs(currentPos - m_stopPos) > 100) {
if (Math.abs(currentPos - m_stopPos) > 200) {
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
} else {
m_drive.driveWithInputAux(0, m_targetGyro);
@@ -145,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2));
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}