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 'add-ramsete-controller-and-trajectories' of https://github.com/Team4388/RiseOfRidgebotics2020 into add-ramsete-controller-and-trajectories

Browse Source
This commit is contained in:
Aarav Shah
2020-02-13 17:17:06 -07:00
parent 9ae3ff611f 4ea924ebe6
commit a280554054
16 changed files with 854 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+45 -7
View File
@@ -7,6 +7,7 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import frc4388.utility.LEDPatterns;
/**
@@ -29,11 +30,17 @@ public final class Constants {
/* PID Constants Drive*/
public static final int DRIVE_TIMEOUT_MS = 30;
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.1, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.5);
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final int DRIVE_CRUISE_VELOCITY = 2000;
public static final int DRIVE_ACCELERATION = 1000;
public static final int DRIVE_CRUISE_VELOCITY = 20000;
public static final int DRIVE_ACCELERATION = 7000;
/* Trajectory Constants */
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kTrackwidthMeters = 0.69; ///TODO: SET THIS SOON!
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters);
/* Remote Sensors */
public final static int REMOTE_0 = 0;
@@ -63,10 +70,24 @@ public final class Constants {
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
public static final double INCHES_PER_METER = 39.370;
}
public static final class IntakeConstants {
public static final int INTAKE_SPARK_ID = 1;
public static final int INTAKE_SPARK_ID = 9;
public static final int EXTENDER_SPARK_ID = 10;
}
public static final class ShooterConstants {
public static final int SHOOTER_FALCON_ID = 8;
/* PID Constants Shooter */
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final int SHOOTER_TIMEOUT_MS = 30;
public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
public static final double ENCODER_TICKS_PER_REV = 2048;
}
public static final class ClimberConstants {
@@ -74,17 +95,34 @@ public final class Constants {
}
public static final class LevelerConstants {
public static final int LEVELER_CAN_ID = 9;
public static final int LEVELER_CAN_ID = -1;
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = -1;
public static final int STORAGE_CAN_ID = 10;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
public static final int BEAM_SENSOR_DIO_1 = 1;
public static final int BEAM_SENSOR_DIO_2 = 2;
public static final int BEAM_SENSOR_DIO_3 = 3;
public static final int BEAM_SENSOR_DIO_4 = 4;
public static final int BEAM_SENSOR_DIO_5 = 5;
/* PID Values */
public static final int SLOT_DISTANCE = 0;
/* PID Indexes */
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 class LEDConstants {
src/main/java/frc4388/robot/Gains.java
+2
View File
@@ -17,6 +17,8 @@ public class Gains {
public double m_kF;
public int m_kIzone;
public double m_kPeakOutput;
public double m_kmaxOutput;
public double m_kminOutput;
/**
* Creates Gains object for PIDs
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -75,6 +75,8 @@ public class Robot extends TimedRobot {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.resetOdometry();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
src/main/java/frc4388/robot/RobotContainer.java
+90 -15
View File
@@ -7,25 +7,40 @@
package frc4388.robot;
import java.util.List;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.commands.ShooterVelocityControlPID;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.LEDPatterns;
@@ -44,6 +59,7 @@ public class RobotContainer {
private final Drive m_robotDrive = new Drive();
private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
@@ -60,13 +76,15 @@ 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()));
// drives motor with input from triggers on the opperator controller
m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
// drives intake with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// 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.runDrumShooter(0.15), m_robotShooter));
// drives the leveler with an axis input from the driver controller
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
// runs storage motor at 50 percent
@@ -81,29 +99,53 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
//new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
// .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
/* 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.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)
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
// turn 45 degrees
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
//new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
// .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive));
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
// interrupts any running command
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
/* Storage Neo PID Test */
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1)));
}
/**
@@ -114,14 +156,47 @@ public class RobotContainer {
m_robotDrive.setDriveTrainNeutralMode(mode);
}
public void resetOdometry() {
m_robotDrive.resetGyroAngles();
m_robotDrive.setOdometry(new Pose2d());
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig( DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(
new Translation2d(1, 1),
new Translation2d(2, -1)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)),
// Pass config
config);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(),
DriveConstants.kDriveKinematics,
m_robotDrive::tankDriveVelocity,
m_robotDrive);
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
}
/**
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+70
View File
@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionMM extends CommandBase {
Drive m_drive;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
return true;
} else {
if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
isGoneFast = true;
}
return false;
}
}
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+15 -6
View File
@@ -14,8 +14,10 @@ import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_targetPos;
double m_targetPosIn;
double m_targetPosOut;
double m_targetGyro;
int i;
/**
* Creates a new DriveToDistancePID.
@@ -25,22 +27,27 @@ public class DriveStraightToPositionPID extends CommandBase {
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
SmartDashboard.putNumber("Distance Target Inches", targetPos);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//System.err.println("PID START \n | \n |");
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
@@ -51,9 +58,11 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
return true;
} else {
i++;
//System.err.println(i);
return false;
}
}
src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java
+79
View File
@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* 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 com.ctre.phoenix.motorcontrol.TalonFXControlMode;
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.utility.controller.IHandController;
public class DriveWithJoystickAuxPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
IHandController m_controller;
/**
* Creates a new DriveWithJoystickAuxPID.
*/
public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
lastTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
if (moveInput >= 0){
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp(m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
System.err.println("Target: " + m_targetGyro);
System.err.println("Current: " + currentGyro);
System.err.println();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+114
View File
@@ -0,0 +1,114 @@
/*----------------------------------------------------------------------------*/
/* 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.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
IHandController m_controller;
boolean isAuxPIDEnabled = false;
/**
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
lastTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
/* If AuxPID is enabled, then update using the steer input */
if (isAuxPIDEnabled) {
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
}
/* Otherwise set target angle to current angle (prevents buildup of gyro error) */
else {
m_targetGyro = currentGyro;
}
/* If move stick is being used */
if (moveInput != 0) {
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
/* If steer stick is being used. */
if (steerInput != 0) {
double cosMultiplier = .45;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steerInput > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(moveOutput, steerOutput);
isAuxPIDEnabled = false;
}
/* If only the move stick is being used */
else {
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
isAuxPIDEnabled = true;
}
}
/* If the move stick is not being used */
else {
m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
isAuxPIDEnabled = true;
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveToDistanceMM.java → src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+24 -22
View File
@@ -8,53 +8,55 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.IHandController;
public class RunExtenderOutIn extends CommandBase {
private Intake m_intake;
private boolean isOut = false;
private long startTime;
public class DriveToDistanceMM extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
double m_rightTarget;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param distance distance to travel in inches
* Uses input from opperator to run the extender motor.
* The left bumper will run the extender in and out.
* @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveToDistanceMM(Drive subsystem, double distance) {
public RunExtenderOutIn(Intake subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
m_intake = subsystem;
addRequirements(m_intake);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
isOut = !isOut;
startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
//m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
if (isOut){
m_intake.runExtender(0.3);
} else {
m_intake.runExtender(-0.3);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_intake.runExtender(0.0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
if (startTime + 3000 < System.currentTimeMillis()) {
return true;
} else {
return false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+48
View File
@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* 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.CommandBase;
import frc4388.robot.subsystems.Shooter;
public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
/**
* Creates a new ShooterVelocityControlPID.
*/
public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_shooter = subsystem;
m_targetVel = targetVel;
addRequirements(m_shooter);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
+9 -6
View File
@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,18 +18,20 @@ import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_forwardLimit, m_reverseLimit;
CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
/**
* Creates a new Climber.
*/
public Climber() {
m_climberMotor.restoreFactoryDefaults();
m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_forwardLimit.enableLimitSwitch(false);
m_reverseLimit.enableLimitSwitch(false);
m_climberMotor.setIdleMode(IdleMode.kBrake);
m_climberMotor.setInverted(false);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_climberForwardLimit.enableLimitSwitch(false);
m_climberReverseLimit.enableLimitSwitch(false);
}
@Override
src/main/java/frc4388/robot/subsystems/Drive.java
+196 -148
View File
@@ -7,6 +7,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
@@ -20,15 +21,18 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Gains;
@@ -52,8 +56,14 @@ public class Drive extends SubsystemBase {
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
public final DifferentialDriveOdometry m_odometry;
public DoubleSolenoid speedShift;
public long m_lastTime, m_deltaTime; //in milliseconds
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
/**
* Add your docs here.
@@ -69,6 +79,8 @@ public class Drive extends SubsystemBase {
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
speedShift = new DoubleSolenoid(7,0,1);
/* set back motors as followers */
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
@@ -92,76 +104,74 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
/* Setup Sensors for WPI_TalonFXs */
resetEncoders();
/* Configure the left Talon's selected sensor as local QuadEncoder */
m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/*
* m_rightFrontMotor.configSelectedFeedbackSensor(
* FeedbackDevice.IntegratedSensor, // Local Feedback Source
* DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
* DriveConstants.DRIVE_TIMEOUT_MS);
*/ // Configuration Timeout
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/*
* Configure the Remote Talon's selected sensor as a remote sensor for the right
* Talon
*/
m_rightFrontMotor.configRemoteFeedbackFilter(m_leftFrontMotor.getDeviceID(), // Device ID of Source
RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/*
* Configure the Pigeon IMU to the other Remote Slot available on the right
* Talon
*/
m_rightFrontMotor.configRemoteFeedbackFilter(m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
/* Setup Sum signal to be used for Distance */
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Diff Signal */
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient(1, // Coefficient
DriveConstants.PID_PRIMARY, // PID Slot of Source
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
DriveConstants.PID_PRIMARY, // PID Slot of Source
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -178,34 +188,16 @@ public class Drive extends SubsystemBase {
/* Smart Dashboard Initial Values */
/* Set up Chooser */
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
// setDriveTrainGains("Distance PID", m_gainsDistance);
m_chooser.addOption("Velocity PID", m_gainsVelocity);
// setDriveTrainGains("Velocity PID", m_gainsVelocity);
m_chooser.addOption("Turning PID", m_gainsTurning);
// setDriveTrainGains("Turning PID", m_gainsTurning);
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
// setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
Shuffleboard.getTab("PID").add(m_chooser);
/* Gyro */
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
/* Sensor Values */
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
/* PID */
Gains gains = m_chooser.getSelected();
Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP);
Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI);
Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD);
Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF);
//SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
//SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
/**
* Max out the peak output (for all modes). However you can limit the output of
@@ -236,45 +228,51 @@ public class Drive extends SubsystemBase {
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
m_lastTime = System.currentTimeMillis();
}
@Override
public void periodic() {
m_deltaTime = System.currentTimeMillis() - m_lastTime;
m_lastTime = System.currentTimeMillis();
m_lastAngleYaw = m_currentAngleYaw;
m_currentAngleYaw = getGyroYaw();
try {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Motor Velocity Int Sensor",
m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor Velocity Int Sensor",
m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
//SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
//SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
//SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
} catch (Exception e) {
System.err.println("Error in the Drive Subsystem");
// e.printStackTrace(System.err);
}
m_odometry.update(Rotation2d.fromDegrees(getHeading()),
m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(),
m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
/**
@@ -290,67 +288,30 @@ public class Drive extends SubsystemBase {
}
/**
* Initializes the drive train gains kP, kI, kD, and kF
*
* @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or
* "Turning PID"
* @param gains A gains object which is the gains that are set for the slot
*/
public void setDriveTrainGains(String slot, Gains gains) {
/* Distance */
if (slot.equals("Distance PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Velocity */
if (slot.equals("Velocity PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Turning */
if (slot.equals("Turning PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput,
DriveConstants.DRIVE_TIMEOUT_MS);
}
/* Motion Magic */
if (slot.equals("Motion Magic PID")) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
}
}
/**
* Add your docs here.
* Runs percent output control on the moving and steering of the drive train,
* using the Differential Drive class to manage the two inputs
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Runs a position PID while driving straight (has not been tested)
*
* Runs percent output control on the drive train while using an AUX PID for rotation
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void driveWithInputAux(double move, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
/**
* Runs position PID while driving straight.
* Position is absolute and displacement should be handled on the command side.
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
@@ -358,7 +319,6 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetPos *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
@@ -374,8 +334,6 @@ public class Drive extends SubsystemBase {
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetVel *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
@@ -383,16 +341,15 @@ public class Drive extends SubsystemBase {
}
/**
* Runs motion magic PID while driving straight (has not been tested)
*
* @param targetPos The position to drive to in units
* Runs motion magic PID while driving straight
* @param targetPos The position to drive to in units
* @param targetGyro The angle to drive at in units
*/
public void runMotionMagicPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
@@ -409,6 +366,29 @@ public class Drive extends SubsystemBase {
runDriveStraightVelocityPID(0, targetGyro);
}
/**
* Controls the left and right sides of the drive with velocity targets.
*
* @param leftSpeed the commanded left output
* @param rightSpeed the commanded right output
*/
public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
double angleVelDeg = Math.toDegrees(angleVelRad);
m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
m_currentAngleYaw-(360),
m_currentAngleYaw+(360));
double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
double moveVel = inchesToMeters(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME;
runDriveStraightVelocityPID(moveVel, targetGyro);
}
/**
* Returns the current yaw of the pigeon
*/
@@ -445,17 +425,36 @@ public class Drive extends SubsystemBase {
public void resetGyroYaw() {
m_pigeon.setYaw(0);
m_pigeon.setAccumZAngle(0);
resetGyroAngles();
}
/**
* Add docs here
*/
public void resetGyroAngles() {
m_lastAngleYaw = 0;
m_currentAngleYaw = 0;
m_kinematicsTargetAngle = 0;
}
/**
* Returns the heading of the robot
*
* @return The robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(getGyroYaw(), 360);
}
/**
* Returns the turn rate of the robot.
*
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
return deltaYaw / (m_deltaTime/1000);
}
/**
* Returns the currently-estimated pose of the robot.
* @return The pose.
@@ -469,8 +468,8 @@ public class Drive extends SubsystemBase {
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds( m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(),
m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftFrontMotor)),
inchesToMeters(getVelocityInchesPerSecond(m_rightFrontMotor)));
}
/**
@@ -500,6 +499,15 @@ public class Drive extends SubsystemBase {
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
}
/**
* Gets the encoder value (velocity) of a motor
* @param falcon The motor to get the velocity of
* @return The velocity of the motor in inches per second
*/
public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
}
/**
* Converts a value in ticks to inches.
* @param ticks The value in ticks to convert
@@ -508,4 +516,44 @@ public class Drive extends SubsystemBase {
public double ticksToInches(double ticks) {
return ticks * DriveConstants.INCHES_PER_TICK;
}
}
/**
* Converts a value in inches to ticks.
* @param inches The value in inches to convert
* @return The converted value in ticks
*/
public double inchesToTicks(double inches) {
return inches * DriveConstants.TICKS_PER_INCH;
}
/**
* Converts a value in inches to meters.
* @param inches The value in inches to convert
* @return The converted value in meters
*/
public double inchesToMeters(double inches) {
return inches / DriveConstants.INCHES_PER_METER;
}
/**
* Converts a value in meters to inches.
* @param meters The value in meters to convert
* @return The converted value in inches
*/
public double metersToInches(double meters) {
return meters * DriveConstants.INCHES_PER_METER;
}
/*
* Set to high or low gear based on boolean state, true = high, false = low
* @param state Chooses between high or low gear
*/
public void setShiftState(boolean state) {
if (state == true) {
speedShift.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
speedShift.set(DoubleSolenoid.Value.kReverse);
}
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+30 -3
View File
@@ -7,19 +7,38 @@
package frc4388.robot.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
public class Intake extends SubsystemBase {
Spark m_intakeMotor = new Spark(IntakeConstants.INTAKE_SPARK_ID);
CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_extenderForwardLimit;
CANDigitalInput m_extenderReverseLimit;
/**
* Creates a new Intake.
*/
public Intake() {
m_intakeMotor.restoreFactoryDefaults();
m_extenderMotor.restoreFactoryDefaults();
m_intakeMotor.setIdleMode(IdleMode.kCoast);
m_extenderMotor.setIdleMode(IdleMode.kBrake);
m_intakeMotor.setInverted(false);
m_extenderMotor.setInverted(false);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
}
@Override
@@ -34,4 +53,12 @@ public class Intake extends SubsystemBase {
public void runIntake(double input) {
m_intakeMotor.set(input);
}
/**
* Runs extender motor
* @param input the percent output to run motor at
*/
public void runExtender(double input) {
m_extenderMotor.set(input);
}
}
src/main/java/frc4388/robot/subsystems/Leveler.java
+2 -1
View File
@@ -26,7 +26,8 @@ public class Leveler extends SubsystemBase {
*/
public Leveler() {
m_levelerMotor.restoreFactoryDefaults();
m_levelerMotor.setIdleMode(IdleMode.kCoast);
m_levelerMotor.setIdleMode(IdleMode.kBrake);
m_levelerMotor.setInverted(false);
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+89
View File
@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.ShooterConstants;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
double velP;
/**
* Creates a new Shooter subsystem.
*/
public Shooter() {
m_shooterFalcon.configFactoryDefault();
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(false);
setShooterGains();
m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
int closedLoopTimeMs = 1;
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
*/
public void runDrumShooter(double speed) {
m_shooterFalcon.set(speed);
}
/**
* Configures gains for shooter PID.
*/
public void setShooterGains() {
m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* Runs drum shooter velocity PID.
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
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
}
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+39 -3
View File
@@ -9,11 +9,15 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.SparkMax;
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.Constants.StorageConstants;
@@ -21,10 +25,17 @@ import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput[] m_beamSensors = new DigitalInput[6];
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
CANEncoder m_encoder = m_storageMotor.getEncoder();
/**
* Creates a new Storage.
*/
public Storage() {
resetEncoder();
m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -40,17 +51,42 @@ public class Storage extends SubsystemBase {
/**
* Runs storage motor
*
* @param input the voltage to run motor at
*/
public void runStorage(double input) {
public void runStorage(final double input) {
m_storageMotor.set(input);
boolean beam_on = m_beamSensors[0].get();
final boolean beam_on = m_beamSensors[0].get();
if (beam_on) {
System.err.println("Beam on");
} else {
System.err.println("Beam off");
}
}
public void resetEncoder()
{
m_encoder.setPosition(0);
}
/* Storage PID Control */
public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
{
// 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.setReference(targetPos, ControlType.kPosition);
}
public void getEncoderPos()
{
m_encoder.getPosition();
}
}