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 auto-programming

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-26 04:45:52 +00:00
committed by GitHub
parent 08d543ba74 2a3655758e
commit 606168599c
23 changed files with 956 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+30 -19
View File
@@ -31,12 +31,19 @@ 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.1, 0.0, 1.0, 0.0, 0, 0.3);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.45);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45);
//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 = 20000;
//public static final int DRIVE_ACCELERATION = 7000;
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.05, 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 = 20000;
public static final int DRIVE_ACCELERATION = 7000;
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 int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
public static final int DRIVE_ACCELERATION_HIGH = 7000;
/* Trajectory Constants */
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
@@ -60,20 +67,27 @@ public final class Constants {
/* Drive Train Characteristics */
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
public static final double WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
/* Ratio Calculation */
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double TICK_TIME_TO_SECONDS = 0.1;
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
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 double METERS_PER_INCH = 1/INCHES_PER_METER;
public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1/MOTOR_ROT_PER_WHEEL_ROT_HIGH;
public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH;
public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK_HIGH = 1/TICKS_PER_INCH_HIGH;
public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1/MOTOR_ROT_PER_WHEEL_ROT_LOW;
public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW;
public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
}
public static final class IntakeConstants {
@@ -125,13 +139,9 @@ 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 double STORAGE_MIN_OUTPUT = -1.0;
public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class LEDConstants {
@@ -148,6 +158,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/Robot.java
+2 -3
View File
@@ -64,7 +64,6 @@ public class Robot extends TimedRobot {
@Override
public void disabledInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
//m_robotContainer.setDriveGearState(true);
}
@Override
@@ -79,7 +78,7 @@ public class Robot extends TimedRobot {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
//m_robotContainer.setDriveGearState(true);
m_robotContainer.resetOdometry();
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
@@ -106,7 +105,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
src/main/java/frc4388/robot/RobotContainer.java
+41 -41
View File
@@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DrivePositionMPAux;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.DriveStraightToPositionMM;
@@ -41,6 +42,7 @@ import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.ShooterVelocityControlPID;
import frc4388.robot.commands.StorageIntakeGroup;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
@@ -53,6 +55,7 @@ import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.commands.TurnDegrees;
import frc4388.robot.commands.Wait;
import frc4388.robot.commands.storageOutake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Storage;
@@ -93,7 +96,7 @@ public class RobotContainer {
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
m_robotDrive.setDefaultCommand(new DriveWithJoystick(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
@@ -101,13 +104,12 @@ 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()));
// runs storage motor at 50 percent
// m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
@@ -117,53 +119,44 @@ 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.runMotionMagicPID(12, 0), m_robotDrive));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36));
// 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));
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), 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 TurnDegrees(90, 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)
@@ -173,6 +166,14 @@ public class RobotContainer {
/* Storage Neo PID Test */
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new TrackTarget(m_robotShooter));
//Prepares storage for intaking
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
.whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
//Runs storage to outtake
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new storageOutake(m_robotStorage));
}
/**
@@ -236,10 +237,8 @@ public class RobotContainer {
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
if (Constants.SELECTED_AUTO == 0) {
return new InstantCommand();
}
else if (Constants.SELECTED_AUTO == 1) {
if (Constants.SELECTED_AUTO == 1) {
return new SequentialCommandGroup( new Wait(5, m_robotDrive),
new TurnDegrees(45, m_robotDrive),
new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive),
@@ -248,6 +247,7 @@ public class RobotContainer {
}
return new InstantCommand();
//return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
}
/**
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+90
View File
@@ -0,0 +1,90 @@
/*----------------------------------------------------------------------------*/
/* 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.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DrivePositionMPAux extends CommandBase {
Drive m_drive;
double m_cruiseVel;
double m_rampDist;
double m_targetPos;
double m_currentVel;
double m_currentPos;
double m_targetGyro;
double m_targetVel;
double m_rampAcc;
long m_startTime;
long m_rampRate;
/**
* Creates a new DrivePositionMPAux.
*
* @param subsystem The drive subsystem
* @param cruiseVel The target velocity for the motors in units
* @param rampDist The distance before cruise velocity is reached in inches
* @param rampRate The time to reach the cruise velocity in seconds
* @param targetPos The target position
*/
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
m_rampRate = (long) rampRate * 1000;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
//m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currentVel = m_drive.m_rightFrontMotorVel;
m_currentPos = m_drive.m_rightFrontMotorPos;
m_targetPos = m_targetPos + m_currentPos;
m_targetVel = m_currentVel;
m_startTime = System.currentTimeMillis();
m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
}
// Called every m_isRamptime the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentVel = m_drive.m_rightFrontMotorVel;
m_currentPos = m_drive.m_rightFrontMotorPos;
if (System.currentTimeMillis() - m_startTime < m_rampRate) {
// Ramping
m_targetVel += m_rampAcc * m_drive.m_deltaTime;
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
} else if (m_targetPos - m_currentPos > m_rampDist) {
// Cruising
m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
} else {
// Deramp PID
m_drive.runDrivePositionPID(-m_targetPos, 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 (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+7 -2
View File
@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
double m_targetPosOut;
double m_targetGyro;
boolean isGoneFast;
int i;
/**
* Creates a new DriveToDistancePID.
@@ -27,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
isGoneFast = false;
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
//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);
SmartDashboard.putBoolean("MM Run", true);
i++;
}
// Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
SmartDashboard.putBoolean("MM Run", false);
return true;
} else {
if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
isGoneFast = true;
}
return false;
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+1 -1
View File
@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
addRequirements(m_drive);
//SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+6 -4
View File
@@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
double cosMultiplier = .45;
double deadzone = .2;
double cosMultiplier = .55;
double deadzone = .1;
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 - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
steerOutput = 0;
}
m_drive.driveWithInput(moveOutput, steerOutput);
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+34 -14
View File
@@ -24,6 +24,22 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
IHandController m_controller;
boolean m_isInterrupted;
/* Deadassist Constants */
final float stopPosVelCoefLow = 1;
final float stopPosVelCoefHigh = 3;
final float cosMultiplierLow = 0.55f;
final float cosMultiplierHigh = 0.35f;
final float targetAngleCoefLow = 5;
final float targetAngleCoefHigh = 5;
final float gyroVelCoefLow = 1;
final float gyroVelCoefHigh = 3;
/* Deadassist Coeficients */
final float stopPosVelCoef = 1;
final float cosMultiplier = 0.55f;
final float targetAngleCoef = 5;
final float gyroVelCoef = 1;
/**
* 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".
@@ -73,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
m_deadTimeSteer = m_currTime;
}
/* If move stick has been pressed within 1 sec */
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* 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;
}
/* 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 (m_drive.m_isSpeedShiftHigh) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If move stick has been pressed within 1 sec */
else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
@@ -99,7 +119,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runDriveWithInput(double move, double steerInput) {
double cosMultiplier = .45;
double cosMultiplier = .55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -119,7 +139,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
}
private void runStoppedTurn(double steer) {
double cosMultiplier = 0.70;
double cosMultiplier = 0.55;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
@@ -131,7 +151,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
updateGyroTarget(steerOutput);
double currentPos = m_drive.m_rightFrontMotorPos;
if (Math.abs(currentPos - m_stopPos) > 100) {
if (Math.abs(currentPos - m_stopPos) > 200) {
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
} else {
m_drive.driveWithInputAux(0, m_targetGyro);
@@ -145,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+29 -7
View File
@@ -7,14 +7,25 @@
package frc4388.robot.commands;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
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;
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;
/**
* Uses input from opperator to run the extender motor.
@@ -25,19 +36,23 @@ public class RunExtenderOutIn extends CommandBase {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
addRequirements(m_intake);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(false);
m_extenderReverseLimit.enableLimitSwitch(false);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
isOut = !isOut;
startTime = System.currentTimeMillis();
m_intake.isExtended = !m_intake.isExtended;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (isOut){
if (m_intake.isExtended){
m_intake.runExtender(0.3);
} else {
m_intake.runExtender(-0.3);
@@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (startTime + 3000 < System.currentTimeMillis()) {
if (m_intake.isExtended && m_extenderForwardLimit.get() == true){
return true;
}
return false;
else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){
return true;
}
else{
return false;
}
}
}
src/main/java/frc4388/robot/commands/StorageIntakeFinal.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.CommandBase;
import frc4388.robot.subsystems.Storage;
public class StorageIntakeFinal extends CommandBase {
Storage m_storage;
/**
* Creates a new StorageIntakeFinal.
*/
public StorageIntakeFinal(Storage subsystem) {
m_storage = subsystem;
addRequirements(m_storage);
}
// 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() {
if (m_storage.getBeam(1)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
}
}
// 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/StorageIntakeGroup.java
+27
View File
@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* 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.Intake;
import frc4388.robot.subsystems.Storage;
// 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 StorageIntakeGroup extends SequentialCommandGroup {
/**
* Creates a new StorageIntakeGroup.
*/
public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
addCommands(
new storagePrepIntake(m_intake, m_storage),
new storageIntake(m_intake, m_storage),
new StorageIntakeFinal(m_storage));
}
}
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/commands/storageIntake.java
+58
View File
@@ -0,0 +1,58 @@
/*----------------------------------------------------------------------------*/
/* 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.Intake;
import frc4388.robot.subsystems.Storage;
public class storageIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
/**
* Creates a new storageIntake.
*/
public storageIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// 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() {
if (m_storage.getBeam(0)){
m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
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) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_storage.getBeam(0)){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storageOutake.java
+44
View File
@@ -0,0 +1,44 @@
/*----------------------------------------------------------------------------*/
/* 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.Storage;
public class storageOutake extends CommandBase {
Storage m_storage;
/**
* Creates a new storageOutake.
*/
public storageOutake(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
// 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() {
m_storage.runStorage(-0.5);
}
// 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/storagePrepAim.java
+52
View File
@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* 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.Storage;
public class storagePrepAim extends CommandBase {
Storage m_storage;
/**
* Creates a new storagePrepAim.
*/
public storagePrepAim(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
// 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() {
if (m_storage.getBeam(2) == false){
m_storage.runStorage(0.5);
}
else{
m_storage.runStorage(0);
}
}
// 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 (m_storage.getBeam(2)){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/storagePrepIntake.java
+56
View File
@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* 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.Intake;
import frc4388.robot.subsystems.Storage;
public class storagePrepIntake extends CommandBase {
public Intake m_intake;
public Storage m_storage;
/**
* Creates a new storagePrepIntake.
*/
public storagePrepIntake(Intake inSub, Storage storeSub) {
m_intake = inSub;
m_storage = storeSub;
addRequirements(m_intake);
addRequirements(m_storage);
}
// 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() {
if (m_storage.getBeam(1) == false){
m_storage.runStorage(-0.5);
}
else{
m_storage.runStorage(0);
}
}
// 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 (m_storage.getBeam(1)){
return true;
}
return false;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+172 -76
View File
@@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
@@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -71,14 +73,21 @@ public class Drive extends SubsystemBase {
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
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 static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW;
public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW;
public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
public final DifferentialDriveOdometry m_odometry;
public DoubleSolenoid m_speedShift;
public boolean m_isSpeedShiftHigh;
public DoubleSolenoid m_coolFalcon;
SendableChooser<String> m_songChooser = new SendableChooser<String>();
@@ -88,6 +97,11 @@ public class Drive extends SubsystemBase {
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
public double m_totalLeftDistanceInches = 0;
public double m_totalRightDistanceInches = 0;
public double m_lastLeftPosTicks = 0;
public double m_lastRightPosTicks = 0;
/**
* Add your docs here.
*/
@@ -99,7 +113,7 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.configFactoryDefault();
m_pigeon.configFactoryDefault();
resetGyroYaw();
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
new Pose2d(0, 0, new Rotation2d()) );
@@ -114,65 +128,44 @@ public class Drive extends SubsystemBase {
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_rightFrontMotor.setInverted(false);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
setDriveTrainNeutralMode(NeutralMode.Coast);
float rampRate = 0.1f;
m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
//SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01);
//m_rightFrontMotor.configSupplyCurrentLimit(c);
//m_leftFrontMotor.configSupplyCurrentLimit(c);
/* deadbands */
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
//m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
//m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
//m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
/* PID for Front Motor Control in Teleop */
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
//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.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.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.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);
/* PID for Back Motor control in Auto */
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
/* Setup Sensors for WPI_TalonFXs */
resetEncoders();
@@ -320,28 +313,51 @@ public class Drive extends SubsystemBase {
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
m_totalRightDistanceInches += ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastLeftPosTicks);
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
getDistanceInches(m_rightFrontMotor),
-getDistanceInches(m_rightFrontMotor));
try {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
//SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.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 Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
/*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
*/
SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
@@ -350,10 +366,10 @@ public class Drive extends SubsystemBase {
//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());
SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putString("Odometry Values Meters", getPose().toString());
//SmartDashboard.putNumber("Odometry Heading", getHeading());
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
if (currentSong != m_songChooser.getSelected()){
@@ -366,9 +382,74 @@ public class Drive extends SubsystemBase {
// e.printStackTrace(System.err);
}
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftBackMotor)),
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
m_lastRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition();
m_lastLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition();
}
public void setRightMotorGains(boolean isHighGear) {
if (!isHighGear) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.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_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.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);
} else {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.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_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
}
}
/**
@@ -424,7 +505,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
//m_driveTrain.feedWatchdog();
m_driveTrain.feedWatchdog();
}
/**
@@ -442,7 +523,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
//m_driveTrain.feedWatchdog();
m_driveTrain.feedWatchdog();
}
/**
@@ -456,6 +537,7 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
@@ -526,6 +608,7 @@ public class Drive extends SubsystemBase {
* Returns the current yaw of the pigeon
*/
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
@@ -572,9 +655,9 @@ public class Drive extends SubsystemBase {
//lol
//sko
//ridge
/**
//brayden=bad coder
* Returns the heading of the robot
/**
* Returns the heading of the robot
* @return The robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
@@ -605,8 +688,8 @@ public class Drive extends SubsystemBase {
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
return new DifferentialDriveWheelSpeeds( getVelocityInchesPerSecond(m_leftBackMotor),
-getVelocityInchesPerSecond(m_rightBackMotor));
}
/**
@@ -617,6 +700,9 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_totalLeftDistanceInches = 0;
m_totalRightDistanceInches = 0;
}
/**
@@ -653,7 +739,11 @@ public class Drive extends SubsystemBase {
* @return The converted value in inches
*/
public double ticksToInches(double ticks) {
return ticks * DriveConstants.INCHES_PER_TICK;
if (m_isSpeedShiftHigh) {
return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
} else {
return ticks * DriveConstants.INCHES_PER_TICK_LOW;
}
}
/**
@@ -662,7 +752,11 @@ public class Drive extends SubsystemBase {
* @return The converted value in ticks
*/
public double inchesToTicks(double inches) {
return inches * DriveConstants.TICKS_PER_INCH;
if (m_isSpeedShiftHigh) {
return inches * DriveConstants.TICKS_PER_INCH_HIGH;
} else {
return inches * DriveConstants.TICKS_PER_INCH_LOW;
}
}
/**
@@ -704,11 +798,13 @@ public class Drive extends SubsystemBase {
*/
public void setShiftState(boolean state) {
if (state == true) {
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
m_speedShift.set(DoubleSolenoid.Value.kReverse);
}
}
if (state == false) {
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
setRightMotorGains(state);
m_isSpeedShiftHigh = state;
}
/**
src/main/java/frc4388/robot/subsystems/Intake.java
+19 -2
View File
@@ -17,16 +17,21 @@ import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
public class Intake extends SubsystemBase {
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;
public boolean isExtended = false;
/**
* Creates a new Intake.
*/
public Intake() {
m_intakeMotor.restoreFactoryDefaults();
m_extenderMotor.restoreFactoryDefaults();
@@ -59,6 +64,18 @@ public class Intake extends SubsystemBase {
* @param input the percent output to run motor at
*/
public void runExtender(double input) {
m_extenderMotor.set(input);
if (m_extenderForwardLimit.get()) {
isExtended = true;
}
if (m_extenderReverseLimit.get()) {
isExtended = false;
}
if (isExtended == false) {
m_extenderMotor.set(input);
}
if (isExtended == true) {
m_extenderMotor.set(-input);
}
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+38 -1
View File
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
@@ -48,6 +49,12 @@ public class Shooter extends SubsystemBase {
double velP;
double input;
ShooterTables m_shooterTable;
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
/*
* Creates a new Shooter subsystem.
@@ -71,6 +78,18 @@ public class Shooter extends SubsystemBase {
int closedLoopTimeMs = 1;
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterTable = new ShooterTables();
SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
}
@Override
@@ -79,6 +98,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 +131,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
+30 -17
View File
@@ -9,16 +9,19 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.revrobotics.CANDigitalInput;
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 com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.StorageConstants;
public class Storage extends SubsystemBase {
@@ -29,12 +32,16 @@ public class Storage extends SubsystemBase {
CANEncoder m_encoder = m_storageMotor.getEncoder();
Gains storageGains = StorageConstants.STORAGE_GAINS;
Intake m_intake;
/**
* 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);
@@ -53,32 +60,38 @@ public class Storage extends SubsystemBase {
*
* @param input the voltage to run motor at
*/
public void runStorage(final double input) {
public void runStorage(double input) {
m_storageMotor.set(input);
final boolean beam_on = m_beamSensors[0].get();
}
public void resetEncoder()
{
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)
{
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(storageGains.m_kP);
m_storagePIDController.setI(storageGains.m_kI);
m_storagePIDController.setD(storageGains.m_kD);
m_storagePIDController.setIZone(storageGains.m_kIzone);
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
public void getEncoderPos()
{
m_encoder.getPosition();
public double getEncoderPos(){
return m_encoder.getPosition();
}
public boolean getBeam(int id){
return m_beamSensors[id].get();
}
public void setStoragePID(double position){
m_storagePIDController.setReference(position , ControlType.kPosition);
}
}
src/main/java/frc4388/utility/ShooterTables.java
+149
View File
@@ -0,0 +1,149 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-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.utility;
import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* Add your docs here.
*/
public class ShooterTables {
double[][] m_distance = new double[50][3];
double[][] m_angle = new double[50][2];
final int m_columnDistance = 0;
final int m_columnHood = 1;
final int m_columnVel = 2;
final int m_columnAngle = 0;
final int m_columnDisplacement = 1;
int m_distanceLength;
int m_angleLength;
public ShooterTables() {
int lineNum = 0;
File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv");
File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv");
try {
BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV));
BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV));
String line = "";
while ((line = distanceReader.readLine()) != null) {
if (lineNum == 0) {
lineNum ++;
} else {
String[] values = line.split(",");
m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
lineNum ++;
}
}
m_distanceLength = lineNum-1;
lineNum = 0;
while ((line = angleReader.readLine()) != null) {
if (lineNum == 0) {
lineNum ++;
} else {
String[] values = line.split(",");
m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]);
m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]);
lineNum ++;
}
}
m_angleLength = lineNum-1;
} catch (FileNotFoundException e) {
e.printStackTrace();
} catch (IOException e) {
e.printStackTrace();
}
SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]);
SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]);
SmartDashboard.putNumber("m_distanceLength", m_distanceLength);
SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]);
SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]);
SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
}
public double getHood(double distance) {
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;
}
if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
return m_distance[i][m_columnHood];
} else {
if (i >= m_distanceLength) {
i = m_distanceLength - 1;
}
return linearInterpolation(i, distance, m_columnHood, m_distance);
}
}
public double getVelocity(double distance) {
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;
}
if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
return m_distance[i][m_columnVel];
} else {
if (i >= m_distanceLength) {
i = m_distanceLength - 1;
}
return linearInterpolation(i, distance, m_columnVel, m_distance);
}
}
public double getAngleDisplacement(double angle) {
int i = 0;
while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {
i ++;
}
if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) {
return m_angle[i][m_columnDisplacement];
} else {
if (i >= m_angleLength) {
i = m_angleLength - 1;
}
return linearInterpolation(i, angle, m_columnDisplacement, m_angle);
}
}
public double linearInterpolation(int i, double value, int column, double[][] table) {
if (i != 0) {
double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]);
value = slope * (value - table[i-1][0]) + table[i-1][column];
return value;
}
return 0.0;
}
}