Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into add-storage

Browse Source
This commit is contained in:
ryan123rudder
2020-02-21 17:14:01 -07:00
committed by GitHub
parent 7667182b01 73e2d0fd59
commit 427ba060da
42 changed files with 1290 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+42 -16
View File
@@ -7,6 +7,7 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import frc4388.utility.LEDPatterns;
/**
@@ -28,13 +29,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.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_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 = 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.05);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 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 = 20000;
//public static final int DRIVE_ACCELERATION = 7000;
/* Trajectory Constants */
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3;
public static final double TRACK_WIDTH_METERS = 0.648;
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
/* Remote Sensors */
public final static int REMOTE_0 = 0;
public final static int REMOTE_1 = 1;
@@ -50,36 +57,44 @@ public final class Constants {
public final static int SLOT_MOTION_MAGIC = 3;
/* Drive Train Characteristics */
public static final double TICKS_PER_MOTOR_REV = 2048*2;
public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
public static final double WHEEL_DIAMETER_INCHES = 6;
public static final double TICKS_PER_GYRO_REV = 8192;
/* Ratio Calculation */
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_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO;
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 class IntakeConstants {
public static final int INTAKE_SPARK_ID = 9;
public static final int EXTENDER_SPARK_ID = 10;
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;
/* Motor IDs */
public static final int SHOOTER_FALCON_ID = -1;
public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
public static final int SHOOTER_ROTATE_ID = 10;
/* 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 Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double ENCODER_TICKS_PER_REV = 2048;
public static final double NEO_UNITS_PER_REV = 42;
public static final double DEGREES_PER_ROT = 360;
}
public static final class ClimberConstants {
@@ -91,7 +106,7 @@ public final class Constants {
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = 10;
public static final int STORAGE_CAN_ID = -1;
/* Ball Indexes */
public static final int BEAM_SENSOR_DIO_0 = 0;
@@ -116,6 +131,17 @@ public final class Constants {
public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class VisionConstants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 82.75;
public static final double LIME_ANGLE = 18.7366;
public static final double TURN_P_VALUE = 0.65;
public static final double X_ANGLE_ERROR = 1.3;
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 class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
src/main/java/frc4388/robot/Robot.java
+14
View File
@@ -7,9 +7,11 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -34,6 +36,7 @@ public class Robot extends TimedRobot {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
SmartDashboard.putString("Auto?", "NAH");
}
/**
@@ -61,6 +64,7 @@ public class Robot extends TimedRobot {
@Override
public void disabledInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
//m_robotContainer.setDriveGearState(true);
}
@Override
@@ -75,6 +79,10 @@ public class Robot extends TimedRobot {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
m_robotContainer.resetOdometry();
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
@@ -85,6 +93,7 @@ public class Robot extends TimedRobot {
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
System.err.println("Auto Start");
}
}
@@ -98,6 +107,9 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
m_robotContainer.setDriveGearState(true);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -105,6 +117,8 @@ public class Robot extends TimedRobot {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
SmartDashboard.putString("Auto?", "NAH");
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+90 -13
View File
@@ -7,19 +7,34 @@
package frc4388.robot;
import java.util.List;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.Joystick;
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.DriveWithJoystick;
import frc4388.robot.commands.DriveWithJoystickAuxPID;
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
import frc4388.robot.commands.RunIntakeWithTriggers;
@@ -33,6 +48,8 @@ import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.LEDPatterns;
@@ -56,6 +73,10 @@ public class RobotContainer {
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
/* Cameras */
private final Camera m_robotCameraFront = new Camera("front",0,160,120,40);
private final Camera m_robotCameraBack = new Camera("back",1,160,120,40);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -76,9 +97,10 @@ 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.runDrumShooter(0.15), m_robotShooter));
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()));
}
@@ -91,16 +113,19 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
.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.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));
@@ -116,27 +141,33 @@ public class RobotContainer {
// resets the yaw of the pigeon
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
.whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
// turn 45 degrees
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
// sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
// sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_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));
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
/* Storage Neo PID Test */
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
.whileHeld(new TrackTarget(m_robotShooter));
//Prepares storage for intaking
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
@@ -146,7 +177,7 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
}
/**
* Sets Motors to a NeutralMode.
* @param mode NeutralMode to set motors to
@@ -155,13 +186,59 @@ public class RobotContainer {
m_robotDrive.setDriveTrainNeutralMode(mode);
}
/**
* Sets the gear of the drivetrain
* @param state the gearing of the gearbox (true is high, false is low)
*/
public void setDriveGearState(boolean state) {
m_robotDrive.setShiftState(state);
}
public void configDriveTrainSensors(FeedbackDevice type) {
m_robotDrive.configMotorSensor(type);
}
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
// Create config for trajectory
/*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
// 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(10, 0)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(20, 20, new Rotation2d(0)),
// Pass config
config);
// 10 = 20, 20 = 35, 30 = 53.5
// (0,10) = (8,22)
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));*/
return new InstantCommand();
}
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+1 -1
View File
@@ -47,7 +47,7 @@ public class DriveStraightToPositionPID extends CommandBase {
//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);
m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+1 -1
View File
@@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = m_controller.getLeftYAxis();
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+118
View File
@@ -0,0 +1,118 @@
/*----------------------------------------------------------------------------*/
/* 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 DriveWithJoystickDriveStraight extends CommandBase {
Drive m_drive;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
/**
* Creates a new DriveWithJoystickDriveStraight 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 DriveWithJoystickDriveStraight(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_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* 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 has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If steer stick has not been used for 1 sec */
else {
runDriveStraight(moveOutput);
}
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
// 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
+147
View File
@@ -0,0 +1,147 @@
/*----------------------------------------------------------------------------*/
/* 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, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
/**
* 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() {
m_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
/* If move stick is being used */
if (moveInput != 0) {
m_deadTimeMove = m_currTime;
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
}
/* If steer stick is being used */
if (steerInput != 0) {
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;
}
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If steer stick has not been used for 1 sec */
else {
runDriveStraight(moveOutput);
}
}
/* If the move stick has not been used for 1 sec */
else {
runStoppedTurn(steerInput);
}
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
updateGyroTarget(steer);
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
System.out.println("Turning with Target: " + m_targetGyro);
}
/**
* If AuxPID is enabled, then update using the steer input
*/
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
// 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/PlaySongDrive.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.Drive;
public class PlaySongDrive extends CommandBase {
private Drive m_drive;
/**
* Creates a new PlaySongDrive.
*/
public PlaySongDrive(Drive subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.m_rightFrontMotor.set(0);
m_drive.m_leftFrontMotor.set(0);
m_drive.m_rightBackMotor.set(0);
m_drive.m_leftBackMotor.set(0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.playSong();
//System.err.println("Playing " + m_drive.m_orchestra.isPlaying());
//m_drive.m_driveTrain.feedWatchdog();
}
// 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/TrackTarget.java
+84
View File
@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* 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 frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class TrackTarget extends CommandBase {
//Setup
NetworkTableEntry xEntry;
Shooter m_shooter;
IHandController m_driverController;
//Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
/**
* Uses the Limelight to track the target
*/
public TrackTarget(Shooter shooterSubsystem) {
m_shooter = shooterSubsystem;
addRequirements(m_shooter);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
//Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
if (target == 1.0){ //If target in view
//Aiming Left/Right
turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone
//Deadzones
else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;}
else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
m_shooter.runShooterWithInput(turnAmount/5);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
//Drive Camera Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/Camera.java
+47
View File
@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.cscore.MjpegServer;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Camera extends SubsystemBase {
CameraServer camServ = CameraServer.getInstance();
/**
* Creates a new Camera.
* Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
* @param name Name of the Camera in Shuffle Board.
* @param id USB Id of the Camera.
* @param width Resolution width.
* @param height Resolution height.
* @param brightness Percent brightness of the stream.
*/
public Camera(String name, int id, int width, int height, int brightness) {
try{
UsbCamera cam = new UsbCamera(name, id);
cam.setResolution(width, height);
cam.setBrightness(brightness);
cam.setFPS(10);
VideoSource camera = cam;
camServ.startAutomaticCapture(camera);
}
catch(Exception e){
System.err.println("Camera broken, pls nerf");
}
}
@Override
public void periodic() {
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
+30 -7
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,23 @@ 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;
public boolean climberSafety = false;
/**
* 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
@@ -41,6 +47,23 @@ public class Climber extends SubsystemBase {
* @param input the voltage to run motor at
*/
public void runClimber(double input) {
m_climberMotor.set(input);
if(climberSafety){
m_climberMotor.set(input);
}
else{
m_climberMotor.set(0);
}
}
/* Safety Button for Climber */
public void setSafetyPressed()
{
climberSafety = true;
}
/* Safety Button for Climber set back to false */
public void setSafetyNotPressed()
{
climberSafety = false;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+485 -209
View File
@@ -7,6 +7,16 @@
package frc4388.robot.subsystems;
import java.io.File;
import java.io.FilenameFilter;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import java.util.stream.Stream;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -18,15 +28,25 @@ import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
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.BuiltInWidgets;
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;
@@ -42,6 +62,11 @@ public class Drive extends SubsystemBase {
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public Orchestra m_orchestra = new Orchestra();
public double m_rightFrontMotorPos;
public double m_rightFrontMotorVel;
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
@@ -49,9 +74,19 @@ public class Drive extends SubsystemBase {
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_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
public final DifferentialDriveOdometry m_odometry;
public DoubleSolenoid m_speedShift;
public DoubleSolenoid m_coolFalcon;
public DoubleSolenoid speedShift;
SendableChooser<String> m_songChooser = new SendableChooser<String>();
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
public long m_lastTime, m_deltaTime; //in milliseconds
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
/**
* Add your docs here.
@@ -65,31 +100,40 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
speedShift = new DoubleSolenoid(7,0,1);
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
new Pose2d(0, 0, new Rotation2d()) );
m_speedShift = new DoubleSolenoid(7,0,1);
m_coolFalcon = new DoubleSolenoid(7,3,2);
coolFalcon(false);
/* set back motors as followers */
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
setDriveTrainNeutralMode(NeutralMode.Coast);
/* 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_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
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
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);
/* 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);
@@ -97,47 +141,65 @@ public class Drive extends SubsystemBase {
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.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.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);
//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_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);
/* Setup Sensors for WPI_TalonFXs */
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
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 left back Talon's selected sensor as local QuadEncoder */
m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
/* Configure the right back Talon's selected sensor as local QuadEncoder */
m_rightBackMotor.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);
@@ -148,130 +210,170 @@ public class Drive extends SubsystemBase {
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);
/* 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
configMotorSensor(FeedbackDevice.SensorDifference);
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
/*
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);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1,
DriveConstants.PID_TURN,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
//m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
/* Set status frame periods to ensure we don't have stale data */
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
//m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
/* Set status frame periods to ensure we don't have stale data */
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
/* 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());
/* 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);
/**
* Max out the peak output (for all modes).
* However you can limit the output of a given PID object with configClosedLoopPeakOutput().
*/
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* 1ms per loop. PID loop can be slowed down if need be.
* For example,
* - if sensor updates are too slow
* - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
* - sensor movement is very slow causing the derivative error to be near zero.
*/
* Max out the peak output (for all modes). However you can limit the output of
* a given PID object with configClosedLoopPeakOutput().
*/
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* 1ms per loop. PID loop can be slowed down if need be. For example, - if
* sensor updates are too slow - sensor deltas are very small per update, so
* derivative error never gets large enough to be useful. - sensor movement is
* very slow causing the derivative error to be near zero.
*/
int closedLoopTimeMs = 1;
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
/**
* configAuxPIDPolarity(boolean invert, int timeoutMs)
* false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
* true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
}
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
closedLoopTimeMs,
DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN,
closedLoopTimeMs,
DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
closedLoopTimeMs,
DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
closedLoopTimeMs,
DriveConstants.DRIVE_TIMEOUT_MS);
/**
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
* output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
m_lastTime = System.currentTimeMillis();
m_orchestra.addInstrument(m_leftBackMotor);
m_orchestra.addInstrument(m_rightFrontMotor);
m_orchestra.addInstrument(m_rightBackMotor);
m_orchestra.addInstrument(m_leftFrontMotor);
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
String[] songsStrings = songsDir.list();
for (String songString : songsStrings){
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
}
String currentSong = "";
@Override
public void periodic() {
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
if (m_currentTimeSec % 30 == 0) {
coolFalcon(true);
SmartDashboard.putBoolean("Solenoid", true);
} else if ((m_currentTimeSec - 1) % 30 == 0) {
coolFalcon(false);
SmartDashboard.putBoolean("Solenoid", false);
}
m_deltaTime = System.currentTimeMillis() - m_lastTime;
m_lastTime = System.currentTimeMillis();
m_lastAngleYaw = m_currentAngleYaw;
m_currentAngleYaw = getGyroYaw();
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
try {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
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("Pigeon Pitch", getGyroPitch());
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
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 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 Motor Temp", m_rightFrontMotor.getTemperature());
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
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("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("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());
SmartDashboard.putNumber("Odometry Heading", getHeading());
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
if (currentSong != m_songChooser.getSelected()){
currentSong = m_songChooser.getSelected();
selectSong(currentSong);
System.err.println(currentSong);
}
} catch (Exception e) {
System.err.println("Error in the Drive Subsystem");
//e.printStackTrace(System.err);
// e.printStackTrace(System.err);
}
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftBackMotor)),
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
}
/**
* Sets Motors to a NeutralMode.
*
* @param mode NeutralMode to set motors to
*/
public void setDriveTrainNeutralMode(NeutralMode mode) {
@@ -282,120 +384,142 @@ 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
* 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 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);
}
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
}
/**
* Add your docs here.
* 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 driveWithInput(double move, double steer){
m_driveTrain.arcadeDrive(move, steer);
}
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_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs a position PID while driving straight
* @param targetPos The position to drive to in units
* Runs position PID.
* 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
*/
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
public void runDrivePositionPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
* Runs velocity PID while driving straight
* @param targetVel The velocity to drive at in units
* Runs velocity PID
*
* @param targetVel The velocity to drive at in units
* @param targetGyro The angle to drive at in units
*/
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
public void runDriveVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
* Runs motion magic PID while driving straight (has not been tested)
* 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){
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(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs a Turning PID to rotate a to a target angle
*
* @param targetAngle target angle in degrees
*/
public void runTurningPID(double targetAngle) {
double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
runDriveVelocityPID(0, targetGyro);
}
/**
* Controls the left and right sides of the drive with velocity targets.
*
* @param leftSpeed the commanded left speed
* @param rightSpeed the commanded right speed
*/
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 moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
//SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
//SmartDashboard.putNumber("Move Vel Right", moveVelRight);
//runDriveVelocityPID(moveVel*2, targetGyro);
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
System.err.println(moveVelLeft);
m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
m_leftFrontMotor.follow(m_leftBackMotor);
m_rightFrontMotor.follow(m_rightBackMotor);
m_driveTrain.feedWatchdog();
}
/**
* Runs a Turning PID to rotate a to a target angle
* @param targetAngle target angle in degrees
* Selects the feedback device for the motors.
* @param feedbackDevice The feedback device to set it to, usually SensorDifference or
*/
public void runTurningPID(double targetAngle){
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
runDriveStraightVelocityPID(0, targetGyro);
public void configMotorSensor(FeedbackDevice type) {
m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
DriveConstants.DRIVE_TIMEOUT_MS);
}
/**
@@ -404,17 +528,17 @@ public class Drive extends SubsystemBase {
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[0];
}
}
/**
* Returns the current pitch of the pigeon
*/
public double getGyroPitch() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[1];
}
@@ -424,7 +548,7 @@ public class Drive extends SubsystemBase {
*/
public double getGyroRoll() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[2];
}
@@ -435,18 +559,170 @@ 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;
}
//lol
//sko
//ridge
/**
//brayden=bad coder
* 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;
double turnRate = 1000 * deltaYaw / m_deltaTime;
return turnRate;
}
/**
* Returns the currently-estimated pose of the robot.
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/**
* Returns current wheel speeds of robot.
* @return The current wheel speeds.
*/
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
}
/**
* Resets the encoders for both motors.
*/
public void resetEncoders() {
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
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);
}
/**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
public void setOdometry(Pose2d pose) {
resetEncoders();
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
* Gets the encoder value (position) of a motor
* @param falcon The motor to get the position of
* @return The position of the motor in inches
*/
public double getDistanceInches(WPI_TalonFX falcon) {
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
* @return The converted value in inches
*/
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.METERS_PER_INCH;
}
/**
* 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;
}
/*
* Plays Music!
*/
public void playSong() {
m_orchestra.play();
}
/**
* Selects a song to play!
* @param song The name of the song to be played
*/
public void selectSong(String song) {
SmartDashboard.putString("Selected Song", song);
m_orchestra.loadMusic(song);
}
/**
* 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);
m_speedShift.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
speedShift.set(DoubleSolenoid.Value.kReverse);
m_speedShift.set(DoubleSolenoid.Value.kReverse);
}
}
/**
* Set to open or close solenoid that cools the falcon, true = open, false = close
* @param state Chooses between open and close
*/
public void coolFalcon(boolean state) {
if (state == true) {
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
}
if (state == false) {
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
}
}
}
src/main/java/frc4388/robot/subsystems/Leveler.java
+11 -2
View File
@@ -17,16 +17,20 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LevelerConstants;
import frc4388.robot.subsystems.*;
public class Leveler extends SubsystemBase {
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
private final Climber m_robotClimber = new Climber();
/**
* Creates a new Leveler.
*/
public Leveler() {
m_levelerMotor.restoreFactoryDefaults();
m_levelerMotor.setIdleMode(IdleMode.kCoast);
m_levelerMotor.setIdleMode(IdleMode.kBrake);
m_levelerMotor.setInverted(false);
}
@@ -40,6 +44,11 @@ public class Leveler extends SubsystemBase {
* @param input the percent output to run motor at
*/
public void runLeveler(double input) {
m_levelerMotor.set(input);
if(m_robotClimber.climberSafety){
m_levelerMotor.set(input);
}
else{
m_levelerMotor.set(0);
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+89 -10
View File
@@ -11,28 +11,56 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.ControlType;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
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;
import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Shooter m_shooter;
public static IHandController m_controller;
// Configure PID Controllers
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
double velP;
/**
double input;
/*
* Creates a new Shooter subsystem.
*/
public Shooter() {
//Testing purposes reseting gyros
resetGyroAngleAdj();
resetGyroShooterRotate();
m_shooterFalcon.configFactoryDefault();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(true);
m_shooterFalcon.setInverted(false);
setShooterGains();
@@ -64,10 +92,10 @@ public class Shooter extends SubsystemBase {
*/
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);
m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* Runs drum shooter velocity PID.
@@ -87,4 +115,55 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input);
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double targetAngle)
{
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
/* For Testing Purposes, reseting gyro for angle adjuster */
public void resetGyroAngleAdj()
{
m_angleEncoder.setPosition(0);
}
/* For Testing Purposes, reseting gyro for shooter rotation */
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+4 -1
View File
@@ -17,7 +17,6 @@ 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.wpilibj2.command.SubsystemBase;
@@ -61,6 +60,10 @@ public class Storage extends SubsystemBase {
* @param input the voltage to run motor at
*/
public void runStorage(final double input) {
m_storageMotor.set(input);
final boolean beam_on = m_beamSensors[0].get();
}
public void resetEncoder()
{