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

Finished High Gear Velocity and Turning, Working on High Gear Motion Magic

Browse Source
This commit is contained in:
Aarav Shah
2020-03-01 12:33:09 -07:00
parent d8f15ccdb0
commit 26a208524e
4 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -5
View File
@@ -52,16 +52,16 @@ public final class Constants {
/* PID Constants Drive*/
public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY = 30000;
public static final int DRIVE_ACCELERATION = 23000;
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
public static final int DRIVE_ACCELERATION_HIGH = 7000;
src/main/java/frc4388/robot/RobotContainer.java
+2 -2
View File
@@ -119,11 +119,11 @@ public class RobotContainer {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 12));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
.whenPressed(new TurnDegrees(m_robotDrive, 90));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+12 -2
View File
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Pneumatics;
public class DriveStraightToPositionMM extends CommandBase {
Drive m_drive;
Pneumatics m_pneumatics;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
@@ -25,10 +27,18 @@ public class DriveStraightToPositionMM extends CommandBase {
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
try {
if (m_pneumatics.m_isSpeedShiftHigh) {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
} else {
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
}
} catch (Exception e) {
System.err.println("Error In Motion Magic Switch Gains.");
}
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
src/main/java/frc4388/robot/subsystems/Drive.java
+9 -1
View File
@@ -126,7 +126,15 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
/* PID for Front Motor Control in Teleop */
setRightMotorGains(false);
try {
if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
setRightMotorGains(true);
} else {
setRightMotorGains(false);
}
} catch (Exception e) {
System.err.println("Error while trying to switch gains.");
}
/* PID for Back Motor Control in Tank Drive Vel */
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);