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

Reverted Drive code to master

Browse Source
This commit is contained in:
aarav18
2020-02-01 13:38:23 -07:00
parent 43a454f9de
commit 4dd70bf1fe
6 changed files with 6 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -17
View File
@@ -25,12 +25,7 @@ public final class Constants {
public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
public static final int PIGEON_ID = 6;
/* PID Constants Drive*/
public static final int DRIVE_SLOT_IDX = 0;
public static final int DRIVE_PID_LOOP_IDX = 0;
public static final int DRIVE_TIMEOUT_MS = 30;
public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final double ENCODER_TICKS_PER_REV = 2048;
}
@@ -38,18 +33,7 @@ public final class Constants {
public static final int INTAKE_SPARK_ID = 1;
}
public static final class ElevatorConstants {
public static final int TALON_1 = 7;
public static final int TALON_2 = 8;
/* PID Constants Elevator */
public static final int ELEVATOR_SLOT_IDX = 0;
public static final int ELEVATOR_PID_LOOP_IDX = 1;
public static final int ELEVATOR_TIMEOUT_MS = 30;
public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0);
public static final double ENCODER_TICKS_PER_REV = 2048;
}
public static final class ShooterConstants {
public static final int SHOOTER_FALCON_ID = 8;
src/main/java/frc4388/robot/RobotContainer.java
-12
View File
@@ -15,9 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DriveAtVelocityPID;
import frc4388.robot.commands.DriveToDistanceMM;
import frc4388.robot.commands.DriveToDistancePID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.RunIntakeWithTriggers;
import frc4388.robot.commands.ShooterVelocityControlPID;
@@ -81,15 +78,6 @@ public class RobotContainer {
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveToDistancePID(m_robotDrive, 5000));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new DriveToDistanceMM(m_robotDrive, 5000));
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000));
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200));
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 DriveAtVelocityPID extends CommandBase {
Drive m_drive;
double m_targetVel;
double m_leftTarget;
double m_rightTarget;
/**
* Creates a new DriveAtVelocityPID.
*/
public DriveAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_targetVel;
m_rightTarget = -m_targetVel;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runDriveVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveToDistanceMM.java
-57
View File
@@ -1,57 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 DriveToDistanceMM extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
double m_rightTarget;
/**
* Creates a new DriveToDistancePID.
*/
public DriveToDistanceMM(Drive subsystem, double distance) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/commands/DriveToDistancePID.java
-60
View File
@@ -1,60 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
public class DriveToDistancePID extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
double m_rightTarget;
/**
* Creates a new DriveToDistancePID.
*/
public DriveToDistancePID(Drive subsystem, double distance) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
SmartDashboard.putNumber("Left Target", m_leftTarget);
SmartDashboard.putNumber("Right Target", m_rightTarget);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runDrivePositionPID(m_drive.m_rightFrontMotor, m_rightTarget);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+5 -96
View File
@@ -34,8 +34,6 @@ public class Drive extends SubsystemBase {
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
public static Gains m_driveGains = DriveConstants.DRIVE_GAINS;
/**
* Add your docs here.
*/
@@ -53,63 +51,17 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.follow(m_rightFrontMotor);
/* set neutral mode */
setDriveTrainNeutralMode(NeutralMode.Brake);
setDriveTrainNeutralMode(NeutralMode.Coast);
m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE
m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_driveTrain.setRightSideInverted(true);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
/* Motor Encoders */
//m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
//m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
/*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/
setDriveTrainGains();
m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
/* Smart Dashboard Initial Values */
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(0));
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
int closedLoopTimeMs = 1;
m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
}
@Override
@@ -118,23 +70,9 @@ public class Drive extends SubsystemBase {
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));
m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
setDriveTrainGains();
} catch (Exception e) {
System.err.println("The programming team has failed successfully in the Drive Subsystem.");
System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic.");
e.printStackTrace(System.err);
}
}
@@ -149,23 +87,6 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.setNeutralMode(mode);
}
/**
* Add your docs here.
*/
public void setDriveTrainGains(){
m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS);
}
/**
* Add your docs here.
*/
@@ -173,18 +94,6 @@ public class Drive extends SubsystemBase {
m_driveTrain.arcadeDrive(move, steer);
}
public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) {
talon.set(TalonFXControlMode.Position, targetPos);
}
public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) {
talon.set(TalonFXControlMode.Velocity, targetVel);
}
public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){
talon.set(TalonFXControlMode.MotionMagic, targetPos);
}
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);