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 pull request #39 from Team4388/button

Browse Source 
Add Controller Map
This commit is contained in:
Keenan D. Buckley
2020-02-22 02:46:07 +00:00
committed by GitHub
parent 73e2d0fd59 9983b9354b
commit 38a6ab3302
5 changed files with 69 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -6
View File
@@ -123,13 +123,8 @@ public final class Constants {
public static final int PID_PRIMARY = 0;
/* PID Gains */
public static final double storP = 0.1;
public static final double storI = 1e-4;
public static final double storD = 1.0;
public static final double storIz = 0.0;
public static final double storF = 0.0;
public static final double storkmaxOutput = 1.0;
public static final double storkminOutput = -1.0;
public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1);
}
public static final class LEDConstants {
@@ -146,6 +141,7 @@ public final class Constants {
public static final double MOTOR_DEAD_ZONE = 0.3;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
public static final double GRAV = 385.83;
}
public static final class OIConstants {
src/main/java/frc4388/robot/RobotContainer.java
+20 -42
View File
@@ -97,7 +97,6 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the drum shooter in idle mode
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
// drives the leveler with an axis input from the driver controller
// m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
@@ -113,42 +112,7 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
/* Operator Buttons */
// activates "Lit Mode"
//new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
// .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
// .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
/* PID Test Command */
// runs velocity PID while driving straight
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
//new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController()));
// resets the yaw of the pigeon
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
// turn 45 degrees
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
@@ -156,19 +120,33 @@ public class RobotContainer {
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
/* Operator Buttons */
//TODO: Shooter Buttons
// shoots until released
//new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
// shoots one ball
//new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
// aims the turret
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
//.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
// interrupts any running command
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
// extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whenPressed(new RunExtenderOutIn(m_robotIntake));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
/* Storage Neo PID Test */
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
}
/**
src/main/java/frc4388/robot/commands/TrackTarget.java
+13
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
@@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase {
double yAngle = 0;
double target = 0;
public double distance;
public static double fireVel;
public static double fireAngle;
/**
* Uses the Limelight to track the target
@@ -46,6 +49,8 @@ public class TrackTarget extends CommandBase {
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
@@ -65,6 +70,14 @@ public class TrackTarget extends CommandBase {
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
double xVel = (distance*VisionConstants.GRAV)/(yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+22 -1
View File
@@ -48,6 +48,9 @@ public class Shooter extends SubsystemBase {
double velP;
double input;
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
/*
* Creates a new Shooter subsystem.
@@ -79,6 +82,13 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
}
public double addFireVel() {
return m_fireVel;
}
public double addFireAngle() {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
@@ -105,15 +115,26 @@ public class Shooter extends SubsystemBase {
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
velP = actualVel/targetVel; //Percent of target
double runSpeed = actualVel + (12000*velP); //Ramp up equation
//if (runSpeed > targetVel) {runSpeed = targetVel;}
SmartDashboard.putNumber("shoot", actualVel);
runSpeed = runSpeed/targetVel; //Convert to percent
if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
}
else{ //PID Based on targetVel
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
//Tells wether the target velocity has been reached
double upperBound = targetVel + 300;
double lowerBound = targetVel - 300;
if (actualVel < upperBound && actualVel > lowerBound)
{
velReached = true;
}
else{
velReached = false;
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+12 -10
View File
@@ -18,7 +18,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
@@ -29,6 +29,8 @@ public class Storage extends SubsystemBase {
CANEncoder m_encoder = m_storageMotor.getEncoder();
public static Gains m_storageGains = StorageConstants.STORAGE_GAINS;
/**
* Creates a new Storage.
*/
@@ -64,21 +66,21 @@ public class Storage extends SubsystemBase {
}
/* Storage PID Control */
public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
public void runStoragePositionPID(double targetPos)
{
// Set PID Coefficients
m_storagePIDController.setP(kP);
m_storagePIDController.setI(kI);
m_storagePIDController.setD(kD);
m_storagePIDController.setIZone(kIz);
m_storagePIDController.setFF(kF);
m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
m_storagePIDController.setP(m_storageGains.m_kP);
m_storagePIDController.setI(m_storageGains.m_kI);
m_storagePIDController.setD(m_storageGains.m_kD);
m_storagePIDController.setIZone(m_storageGains.m_kIzone);
m_storagePIDController.setFF(m_storageGains.m_kF);
m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput);
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
public void getEncoderPos()
public double getEncoderPos()
{
m_encoder.getPosition();
return m_encoder.getPosition();
}
}