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

Araav Stuff for PID

Browse Source
This commit is contained in:
aarav18
2020-01-31 17:50:43 -07:00
parent e96ccc527a
commit 2d14ef3da3
6 changed files with 39 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -5
View File
@@ -28,12 +28,12 @@ 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, 1.0);
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.3);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
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.05, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.0);
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 = 15000;
public static final int DRIVE_ACCELERATION = 6000;
public static final int DRIVE_CRUISE_VELOCITY = 2000;
public static final int DRIVE_ACCELERATION = 1000;
/* Remote Sensors */
public final static int REMOTE_0 = 0;
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -97,7 +97,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
src/main/java/frc4388/robot/RobotContainer.java
+4 -3
View File
@@ -79,16 +79,17 @@ public class RobotContainer {
/* PID Test Command */
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 0));
.whenPressed(new DriveAtVelocityPID(m_robotDrive, 150))
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotDrive.driveWithInput(0, 0.3), m_robotDrive));
.whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000), m_robotDrive));
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotDrive.getCurrentCommand().cancel(), m_robotDrive));
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
}
/**
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
+3 -2
View File
@@ -42,12 +42,13 @@ public class DriveAtVelocityPID extends CommandBase {
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(m_targetVel, m_targetGyro+1000);
m_drive.runVelocityPID(-20000, m_targetGyro);
SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel);
SmartDashboard.putNumber("Output Target Velocity", m_targetVel);
@@ -60,7 +61,7 @@ public class DriveAtVelocityPID extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.setDriveTrainNeutralMode(NeutralMode.Brake);
m_drive.setDriveTrainNeutralMode(NeutralMode.Coast);
}
// Returns true when the command should end.
src/main/java/frc4388/robot/commands/DriveToDistanceMM.java
+2 -2
View File
@@ -39,8 +39,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.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
//m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Drive.java
+24 -15
View File
@@ -7,6 +7,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
@@ -67,7 +68,7 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.follow(m_rightFrontMotor);
/* set neutral mode */
setDriveTrainNeutralMode(NeutralMode.Brake);
setDriveTrainNeutralMode(NeutralMode.Coast);
/* Setup Sensors for TalonFXs */
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -216,14 +217,17 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Motor Output", m_rightFrontMotor.getMotorOutputPercent());
SmartDashboard.putNumber("Left Motor Output", m_leftFrontMotor.getMotorOutputPercent());
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", 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("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));
Gains gains = m_chooser.getSelected();
if (gains.equals(gainsOld)) {
@@ -318,14 +322,19 @@ public class Drive extends SubsystemBase {
public void runVelocityPID(double targetVel, double targetGyro) {
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.setInverted(false);
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, targetVel, DemandType.AuxPID, targetGyro);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
m_rightFrontMotor.setInverted(true);
m_rightFrontMotor.set(TalonFXControlMode.Velocity, 20000, DemandType.AuxPID, targetGyro);
//m_rightFrontMotor.set(ControlMode.PercentOutput, 0.2);
//m_leftFrontMotor.follow(m_rightFrontMotor);
//m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
}
public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){
talon.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
talon.set(TalonFXControlMode.MotionMagic, targetPos);
public void runMotionMagicPID(double targetPos){
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
m_rightFrontMotor.setInverted(true);
m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos);
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
}
public void runTurningPID(double targetAngle){