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

Finished Elevator Robot Challenge

Browse Source
This commit is contained in:
aarav18
2020-01-18 14:29:30 -08:00
parent d4be141676
commit 101cfb5d10
8 changed files with 105 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8
View File
@@ -41,6 +41,14 @@ public final class Constants {
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.2, 0.0, 0.0, 0.2, 0, 1.0);
public static final double ENCODER_TICKS_PER_REV = 2048;
}
public static final class LEDConstants {
src/main/java/frc4388/robot/RobotContainer.java
+4
View File
@@ -15,6 +15,7 @@ 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.DistanceElevatorPID;
import frc4388.robot.commands.DriveAtVelocityPID;
import frc4388.robot.commands.DriveToDistanceMM;
import frc4388.robot.commands.DriveToDistancePID;
@@ -94,6 +95,9 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new DistanceElevatorPID(m_robotElevator, 20000));
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(null, m_robotDrive));
}
src/main/java/frc4388/robot/commands/DistanceElevatorPID.java
+56
View File
@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Elevator;
public class DistanceElevatorPID extends CommandBase {
Elevator m_elevator;
double m_distance;
double m_target1;
double m_target2;
/**
* Creates a new DistanceElevatorPID.
*/
public DistanceElevatorPID(Elevator subsystem, double distance) {
// Use addRequirements() here to declare subsystem dependencies.
m_elevator = subsystem;
m_distance = distance;
addRequirements(m_elevator);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_target1 = m_elevator.m_talon1.getActiveTrajectoryPosition() + m_distance;
m_target2 = m_elevator.m_talon2.getActiveTrajectoryPosition() + m_distance;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_elevator.runElevatorPositionPID(m_elevator.m_talon1, m_target1);
m_elevator.runElevatorPositionPID(m_elevator.m_talon2, m_target2);
}
// 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_elevator.m_talon1.getActiveTrajectoryPosition() - m_target1) < 100) {
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
+2 -2
View File
@@ -35,8 +35,8 @@ public class DriveAtVelocityPID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget);
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.
src/main/java/frc4388/robot/commands/DriveToDistanceMM.java
+2 -2
View File
@@ -36,8 +36,8 @@ public class DriveToDistanceMM extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
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.
src/main/java/frc4388/robot/commands/DriveToDistancePID.java
+2 -2
View File
@@ -39,8 +39,8 @@ public class DriveToDistancePID extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget);
m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget);
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.
src/main/java/frc4388/robot/subsystems/Drive.java
+3 -3
View File
@@ -170,15 +170,15 @@ public class Drive extends SubsystemBase {
m_driveTrain.arcadeDrive(move, steer);
}
public void runPositionPID(WPI_TalonFX talon, double targetPos) {
public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) {
talon.set(TalonFXControlMode.Position, targetPos);
}
public void runVelocityPID(WPI_TalonFX talon, double targetVel) {
public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) {
talon.set(TalonFXControlMode.Velocity, targetVel);
}
public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){
public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){
talon.set(TalonFXControlMode.MotionMagic, targetPos);
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+28
View File
@@ -7,11 +7,13 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.ElevatorConstants;
public class Elevator extends SubsystemBase {
@@ -19,13 +21,16 @@ public class Elevator extends SubsystemBase {
public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1);
public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2);
public static Gains m_gains = ElevatorConstants.ELEVATOR_GAINS;
/**
* Creates a new Elevator.
*/
public Elevator() {
//resets motors
m_talon1.configFactoryDefault();
m_talon2.configFactoryDefault();
//config following settings
m_talon2.follow(m_talon1);
m_talon1.setNeutralMode(NeutralMode.Brake);
@@ -35,6 +40,13 @@ public class Elevator extends SubsystemBase {
m_talon2.setInverted(false);
m_talon1.setInverted(InvertType.FollowMaster);
m_talon2.setInverted(InvertType.FollowMaster);
m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
int closedLoopTimeMs = 1;
m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
}
@Override
@@ -45,4 +57,20 @@ public class Elevator extends SubsystemBase {
m_talon1.set(speed);
m_talon2.set(speed);
}
public void setElevatorGains(){
m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX);
m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX);
m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_gains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
}
public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) {
talon.set(ControlMode.Position, targetPos);
}
}