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

Tweaks to Deadassist and Work on Motion Magic

Browse Source
This commit is contained in:
Aarav Shah
2020-02-21 20:46:52 -07:00
parent 9f1aff6c8d
commit 5f989fb21a
6 changed files with 27 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+6 -1
View File
@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
int i;
/**
* Creates a new DriveToDistancePID.
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
SmartDashboard.putBoolean("MM Run", true);
i++;
}
// Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
isGoneFast = true;
}
return false;
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+1 -1
View File
@@ -48,7 +48,7 @@ public class DriveWithJoystick extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
double cosMultiplier = .45;
double cosMultiplier = .55;
double deadzone = .2;
if (steerInput > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+4 -4
View File
@@ -99,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runDriveWithInput(double move, double steerInput) {
double cosMultiplier = .45;
double cosMultiplier = .55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -119,7 +119,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runStoppedTurn(double steer) {
double cosMultiplier = 0.70;
double cosMultiplier = 0.55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -145,8 +145,8 @@ 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/4),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2));
}
/**