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

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/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.