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

Finished Drive Straight Velocity PID and started Position PID

Browse Source
This commit is contained in:
aarav18
2020-02-06 19:43:37 -07:00
parent 987a3a2647
commit 92c4a18c6e
5 changed files with 32 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+2 -4
View File
@@ -15,7 +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.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
@@ -68,9 +67,8 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
// test command to spin the robot while pressing A on the driver controller
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(() -> m_robotDrive.driveWithInput(0, 1), m_robotDrive);
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
/* Operator Buttons */
// activates "Lit Mode"
@@ -80,7 +78,7 @@ public class RobotContainer {
/* PID Test Command */
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000))
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
-72
View File
@@ -1,72 +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 com.ctre.phoenix.motorcontrol.FollowerType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class DriveAtVelocityPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
double m_targetVel;
double m_leftTarget;
double m_rightTarget;
double m_copiedTargetVel;
/**
* Creates a new DriveAtVelocityPID.
* @param subsystem drive subsystem
* @param distance target velocity in inches/second
*/
public DriveAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;
m_copiedTargetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_targetVel;
m_rightTarget = m_targetVel;
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
m_drive.setDriveTrainNeutralMode(NeutralMode.Coast);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runVelocityPID(-20000, m_targetGyro);
SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel);
SmartDashboard.putNumber("Output Target Velocity", m_targetVel);
SmartDashboard.putNumber("Target Angle", m_targetGyro);
//m_drive.runVelocityPID(m_leftTarget);
//m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.setDriveTrainNeutralMode(NeutralMode.Coast);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -35,7 +35,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
@Override
public void execute() {
System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runVelocityPID(m_targetVel, m_targetGyro);
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+10 -15
View File
@@ -14,38 +14,33 @@ import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
double m_rightTarget;
double m_targetPos;
double m_targetGyro;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param distance distance to travel in inches
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionPID(Drive subsystem, double distance) {
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance * DriveConstants.TICKS_PER_INCH;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
SmartDashboard.putNumber("Distance Target Inches", distance);
SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// 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("Distance Target Ticks", m_distance);
SmartDashboard.putNumber("Left Target Ticks", m_leftTarget);
SmartDashboard.putNumber("Right Target Ticks", m_rightTarget);
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
}
// 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);
System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
}
// Called once the command ends or is interrupted.
@@ -56,7 +51,7 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
return true;
} else {
return false;
src/main/java/frc4388/robot/subsystems/Drive.java
+19 -8
View File
@@ -93,7 +93,14 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kF, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kP, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
/* Setup Sensors for WPI_TalonFXs */
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -313,21 +320,25 @@ public class Drive extends SubsystemBase {
m_driveTrain.arcadeDrive(move, steer);
}
public void runPositionPID(WPI_TalonFX talon, double targetPos) {
talon.selectProfileSlot(DriveConstants.SLOT_DISTANCE , DriveConstants.PID_PRIMARY);
talon.set(TalonFXControlMode.Position, targetPos);
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetPos *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
long last = 0;
public void runVelocityPID(double targetVel, double targetGyro) {
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
targetVel *= 2;
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
//m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_driveTrain.feedWatchdog();
}
@@ -345,7 +356,7 @@ public class Drive extends SubsystemBase {
*/
public void runTurningPID(double targetAngle){
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
runVelocityPID(0, targetGyro);
runDriveStraightVelocityPID(0, targetGyro);
}
public double getGyroYaw() {