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

Merge branch 'master' into master-updates

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-03 01:04:56 +00:00
committed by GitHub
parent 923c04bf34 faae8cfa6a
commit a6e5103193
13 changed files with 267 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+11 -9
View File
@@ -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;
src/main/java/frc4388/robot/Robot.java
+2 -2
View File
@@ -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
src/main/java/frc4388/robot/RobotContainer.java
+13 -29
View File
@@ -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();
}
}
}
src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+45
View File
@@ -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
);
}
}
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+46
View File
@@ -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)
);
}
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+13 -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,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);
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+13 -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 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);
}
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+33 -14
View File
@@ -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);
}
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+35 -8
View File
@@ -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.
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+4 -4
View File
@@ -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);
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+25 -5
View File
@@ -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));
}
src/main/java/frc4388/robot/commands/Wait.java
+12 -1
View File
@@ -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.
src/main/java/frc4388/robot/subsystems/Drive.java
+15 -2
View File
@@ -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);