mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-08 16:28:01 -06:00
Merge branch 'master' into master-updates
This commit is contained in:
Keenan D. Buckley
@@ -42,6 +42,8 @@ public final class Constants {
|
||||
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
|
||||
new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
|
||||
public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
|
||||
public static final double COS_MULTIPLIER_LOW = 1.0;
|
||||
public static final double COS_MULTIPLIER_HIGH = 0.8;
|
||||
|
||||
/* Drive Train Characteristics */
|
||||
public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
|
||||
@@ -52,16 +54,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_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final int DRIVE_CRUISE_VELOCITY = 25000;
|
||||
public static final int DRIVE_ACCELERATION = 21000;
|
||||
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_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
|
||||
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;
|
||||
|
||||
|
||||
@@ -77,7 +77,7 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
|
||||
//m_robotContainer.setDriveGearState(true);
|
||||
m_robotContainer.setDriveGearState(false);
|
||||
m_robotContainer.resetOdometry();
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
|
||||
//m_robotContainer.setDriveGearState(true);
|
||||
m_robotContainer.setDriveGearState(false);
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
|
||||
@@ -29,12 +29,15 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.AutoPath1FromCenter;
|
||||
import frc4388.robot.commands.AutoPath2FromRight;
|
||||
import frc4388.robot.commands.CalibrateShooter;
|
||||
import frc4388.robot.commands.DrivePositionMPAux;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
|
||||
@@ -123,7 +126,10 @@ public class RobotContainer {
|
||||
|
||||
/* Default Commands */
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
|
||||
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
|
||||
//m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
|
||||
|
||||
// drives intake with input from triggers on the opperator controller
|
||||
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
|
||||
// runs the turret with joystick
|
||||
@@ -151,15 +157,15 @@ public class RobotContainer {
|
||||
/* Test Buttons */
|
||||
// A driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192));
|
||||
.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)
|
||||
.whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
|
||||
.whenPressed(new Wait(m_robotDrive, 0, 0));
|
||||
|
||||
// X driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
@@ -249,31 +255,9 @@ public class RobotContainer {
|
||||
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
|
||||
// Run path following command, then stop at the end.
|
||||
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
|
||||
//Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
|
||||
//This assumes that we are positioned against the right wall with our shooter facing the target.
|
||||
return new SequentialCommandGroup(new Wait(0, m_robotDrive),
|
||||
//add aim command
|
||||
//add shooter command
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 48.0),
|
||||
/*new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
|
||||
new ParallelCommandGroup(
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
|
||||
new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/
|
||||
//add aim command
|
||||
//add shooter command
|
||||
//Below this would be the picking up additional balls outside of those in the trench directly behind us
|
||||
return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
|
||||
//return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);
|
||||
|
||||
//new GotoCoordinates(m_robotDrive, 36, 36),
|
||||
new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
|
||||
//new StorageIntakeGroup(m_robotIntake, m_robotStorage),
|
||||
//new TurnDegrees(m_robotDrive, 75),
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 18.0),
|
||||
//new TurnDegrees(m_robotDrive, -45),
|
||||
//new DriveStraightToPositionMM(m_robotDrive, 12.0));
|
||||
}
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
return new TrajectoryConfig(
|
||||
@@ -373,4 +357,4 @@ public class RobotContainer {
|
||||
{
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
|
||||
public class AutoPath1FromCenter extends SequentialCommandGroup {
|
||||
Drive m_drive;
|
||||
Pneumatics m_pneumatics;
|
||||
|
||||
/**
|
||||
* Creates a new AutoPath1FromCenter.
|
||||
*/
|
||||
public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = subsystem2;
|
||||
|
||||
addCommands( new Wait(m_drive, 0, 1),
|
||||
//shoot pre-loaded 3 balls
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90),
|
||||
//Start Intake Ball 1
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 12),
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
|
||||
//Start Intake Ball 2
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
|
||||
//Start Intake Ball 3
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
|
||||
new Wait(m_drive, 0, 2)
|
||||
//Shoot 3 Balls
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
|
||||
public class AutoPath2FromRight extends SequentialCommandGroup {
|
||||
Drive m_drive;
|
||||
Pneumatics m_pneumatics;
|
||||
|
||||
/**
|
||||
* Creates a new AutoPath2FromRight.
|
||||
*/
|
||||
public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = subsystem2;
|
||||
|
||||
addCommands( new Wait(m_drive, 0, 1),
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 77),
|
||||
//Start Intake Ball 1
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
|
||||
//Start Intake Ball 2
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
|
||||
//Shoot 5 Balls
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
|
||||
//Start Intake Ball 1 (second round)
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
|
||||
//Start Moving to 4th Ball
|
||||
new GotoCoordinates(m_drive, m_pneumatics, 60, -50),
|
||||
new Wait(m_drive, 0, 2)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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,19 @@ 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;
|
||||
m_pneumatics = subsystem2;
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -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 DriveStraightToPositionPID extends CommandBase {
|
||||
Drive m_drive;
|
||||
Pneumatics m_pneumatics;
|
||||
double m_targetPosIn;
|
||||
double m_targetPosOut;
|
||||
double m_targetGyro;
|
||||
@@ -24,10 +26,19 @@ public class DriveStraightToPositionPID extends CommandBase {
|
||||
* @param subsystem drive subsystem
|
||||
* @param targetPos distance to travel in inches
|
||||
*/
|
||||
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
|
||||
public DriveStraightToPositionPID(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;
|
||||
m_pneumatics = subsystem2;
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -9,12 +9,15 @@ package frc4388.robot.commands;
|
||||
|
||||
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;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystick extends CommandBase {
|
||||
private Drive m_drive;
|
||||
private IHandController m_controller;
|
||||
private Pneumatics m_pneumatics;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
|
||||
@@ -24,9 +27,10 @@ public class DriveWithJoystick extends CommandBase {
|
||||
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
|
||||
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
*/
|
||||
public DriveWithJoystick(Drive subsystem, IHandController controller) {
|
||||
public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = subsystem2;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
@@ -49,8 +53,15 @@ public class DriveWithJoystick extends CommandBase {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
double cosMultiplier = 1.0;
|
||||
double cosMultiplier;
|
||||
double deadzone = .1;
|
||||
|
||||
if (m_pneumatics.m_isSpeedShiftHigh) {
|
||||
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
|
||||
} else {
|
||||
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
|
||||
}
|
||||
|
||||
if (steerInput > 0){
|
||||
steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
|
||||
} else if (steerInput < 0) {
|
||||
@@ -58,23 +69,31 @@ public class DriveWithJoystick extends CommandBase {
|
||||
} else {
|
||||
steerOutput = 0;
|
||||
}
|
||||
double tempOutputLimit = 0.8;
|
||||
|
||||
boolean isOutputLimited = false;
|
||||
/*
|
||||
double outputLimit = 0.8;
|
||||
|
||||
if (isOutputLimited) {
|
||||
if (moveOutput > tempOutputLimit) {
|
||||
moveOutput = tempOutputLimit;
|
||||
} else if(moveOutput < -tempOutputLimit) {
|
||||
moveOutput = -tempOutputLimit;
|
||||
boolean isMoveOutputLimited = false;
|
||||
boolean isSteerOutputLimited = false;
|
||||
|
||||
if (m_pneumatics.m_isSpeedShiftHigh) {
|
||||
if (isMoveOutputLimited) {
|
||||
if (moveOutput > outputLimit) {
|
||||
moveOutput = outputLimit;
|
||||
} else if(moveOutput < -outputLimit) {
|
||||
moveOutput = -outputLimit;
|
||||
}
|
||||
}
|
||||
|
||||
if (steerOutput > tempOutputLimit) {
|
||||
steerOutput = tempOutputLimit;
|
||||
} else if(steerOutput < -tempOutputLimit) {
|
||||
steerOutput = -tempOutputLimit;
|
||||
if (isSteerOutputLimited) {
|
||||
if (steerOutput > outputLimit) {
|
||||
steerOutput = outputLimit;
|
||||
} else if(steerOutput < -outputLimit) {
|
||||
steerOutput = -outputLimit;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
m_drive.driveWithInput(moveOutput, steerOutput);
|
||||
}
|
||||
|
||||
@@ -7,20 +7,27 @@
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.security.PublicKey;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
Drive m_drive;
|
||||
Pneumatics m_pneumatics;
|
||||
double m_targetGyro, m_currentGyro;
|
||||
double m_stopPos;
|
||||
long m_currTime, m_deltaTime;
|
||||
long m_deadTimeSteer, m_deadTimeMove;
|
||||
long m_deadTimeout = 100;
|
||||
IHandController m_controller;
|
||||
boolean m_isInterrupted;
|
||||
double highGearMultiplier = 1;
|
||||
double lowGearMultiplier = 1;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
|
||||
@@ -34,6 +41,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = m_drive.m_pneumaticsSubsystem;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
@@ -54,6 +62,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 +91,22 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steer) {
|
||||
double cosMultiplier = .45;
|
||||
|
||||
double cosMultiplier;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steer > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
|
||||
double deadzone = .1;
|
||||
|
||||
if (m_pneumatics.m_isSpeedShiftHigh) {
|
||||
cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
|
||||
cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
|
||||
}
|
||||
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
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");
|
||||
@@ -101,13 +122,19 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
*/
|
||||
private void resetGyroTarget() {
|
||||
//m_targetGyro = m_currentGyro;
|
||||
m_targetGyro = m_currentGyro
|
||||
+ m_drive.getTurnRate();
|
||||
if (m_pneumatics.m_isSpeedShiftHigh) {
|
||||
m_targetGyro = m_currentGyro
|
||||
+ highGearMultiplier * m_drive.getTurnRate();
|
||||
} else {
|
||||
m_targetGyro = m_currentGyro
|
||||
+ lowGearMultiplier * m_drive.getTurnRate();
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_isInterrupted = interrupted;
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
|
||||
@@ -122,14 +122,14 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steerInput) {
|
||||
double cosMultiplier = .55;
|
||||
double cosMultiplier = .70;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
double deadzone = .1;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steerInput > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
|
||||
steerOutput = -(cosMultiplier - deadzone)*Math.cos(1.571*steerInput)+(cosMultiplier);
|
||||
} else if (steerInput < 0) {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
|
||||
steerOutput = (cosMultiplier - deadzone)*Math.cos(1.571*steerInput)-(cosMultiplier);
|
||||
}
|
||||
|
||||
m_drive.driveWithInput(move, steerOutput);
|
||||
|
||||
@@ -10,12 +10,14 @@ package frc4388.robot.commands;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
|
||||
// NOTE: Consider using this command inline, rather than writing a subclass. For more
|
||||
// information, see:
|
||||
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
|
||||
public class GotoCoordinates extends SequentialCommandGroup {
|
||||
Drive m_drive;
|
||||
Pneumatics m_pneumatics;
|
||||
|
||||
double m_xTarget;
|
||||
double m_yTarget;
|
||||
@@ -26,10 +28,11 @@ public class GotoCoordinates extends SequentialCommandGroup {
|
||||
/**
|
||||
* Creates a new GotoPosition.
|
||||
*/
|
||||
public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
|
||||
public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
|
||||
// Add your commands in the super() call, e.g.
|
||||
// super(new FooCommand(), new BarCommand());
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = subsystem2;
|
||||
|
||||
m_xTarget = xTarget;
|
||||
m_yTarget = yTarget;
|
||||
@@ -38,13 +41,30 @@ public class GotoCoordinates extends SequentialCommandGroup {
|
||||
|
||||
m_currentAngle = calcAngle();
|
||||
|
||||
SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle);
|
||||
|
||||
m_endAngle = endAngle;
|
||||
|
||||
addCommands( new TurnDegrees(m_drive, m_currentAngle),
|
||||
new Wait(m_drive, 0, 0),
|
||||
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
|
||||
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
|
||||
}
|
||||
|
||||
addCommands( new TurnDegrees(m_drive, m_currentAngle),
|
||||
new DriveStraightToPositionMM(m_drive, m_hypotDist),
|
||||
public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
|
||||
m_drive = subsystem;
|
||||
m_pneumatics = subsystem2;
|
||||
|
||||
m_xTarget = xTarget;
|
||||
m_yTarget = yTarget;
|
||||
|
||||
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
|
||||
|
||||
m_currentAngle = calcAngle();
|
||||
|
||||
m_endAngle = m_currentAngle;
|
||||
|
||||
addCommands( new TurnDegrees(m_drive, m_currentAngle),
|
||||
new Wait(m_drive, 0, 0),
|
||||
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
|
||||
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
|
||||
}
|
||||
|
||||
|
||||
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
|
||||
long m_waitTime;
|
||||
long m_currentTime;
|
||||
SubsystemBase m_subsystem;
|
||||
int m_waitNum;
|
||||
|
||||
int counter = 0;
|
||||
|
||||
/**
|
||||
* Creates a new WaitCommand.
|
||||
*/
|
||||
public Wait(float seconds, SubsystemBase subsystem) {
|
||||
public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
|
||||
m_waitTime = (long) (seconds * 1000);
|
||||
m_subsystem = subsystem;
|
||||
m_waitNum = waitNum;
|
||||
|
||||
addRequirements(m_subsystem);
|
||||
}
|
||||
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
|
||||
if (counter == 0) {
|
||||
SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
|
||||
}
|
||||
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
|
||||
|
||||
counter ++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -53,7 +53,7 @@ public class Drive extends SubsystemBase {
|
||||
public Orchestra m_orchestra;
|
||||
|
||||
/* Pneumatics Subsystem */
|
||||
Pneumatics m_pneumaticsSubsystem;
|
||||
public Pneumatics m_pneumaticsSubsystem;
|
||||
|
||||
/* Low Gear Gains */
|
||||
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
|
||||
@@ -114,6 +114,11 @@ public class Drive extends SubsystemBase {
|
||||
m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Config Supply Current Limit (Use only for debugging) */
|
||||
//m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
//m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
@@ -127,7 +132,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);
|
||||
|
||||
Reference in New Issue
Block a user