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

Work on Broken PIDs

Browse Source
This commit is contained in:
Aarav Shah
2020-02-26 20:29:01 -07:00
parent afaae3ca1d
commit 3a3d10a382
7 changed files with 31 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -30,7 +30,7 @@ public final class Constants {
public static final int PIGEON_ID = 6;
/* Drive Inversions */
public static final boolean isRightMotorInverted = false;
public static final boolean isRightMotorInverted = true;
public static final boolean isLeftMotorInverted = false;
public static final boolean isRightArcadeInverted = false;
public static final boolean isAuxPIDInverted = false;
@@ -53,7 +53,7 @@ public final class Constants {
/* PID Constants Drive*/
public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = 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;
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
//m_robotContainer.setDriveGearState(true);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
// This makes sure that the autonomous stops running when
src/main/java/frc4388/robot/RobotContainer.java
+5 -3
View File
@@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.DrivePositionMPAux;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
@@ -39,6 +40,7 @@ import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.commands.RunLevelerWithJoystick;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.commands.TurnDegrees;
import frc4388.robot.commands.storageOutake;
import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Leveler;
@@ -109,11 +111,11 @@ public class RobotContainer {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new TurnDegrees(90, m_robotDrive));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
@@ -183,7 +185,7 @@ public class RobotContainer {
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
// return new InstantCommand();
return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
return new DrivePositionMPAux(m_robotDrive, 12.0, 12.0, 2, 100.0);
}
TrajectoryConfig getTrajectoryConfig() {
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+3 -3
View File
@@ -29,12 +29,12 @@ public class DrivePositionMPAux extends CommandBase {
* Creates a new DrivePositionMPAux.
*
* @param subsystem The drive subsystem
* @param cruiseVel The target velocity for the motors in units
* @param cruiseVel The target velocity for the motors in in/s
* @param rampDist The distance before cruise velocity is reached in inches
* @param rampRate The time to reach the cruise velocity in seconds
* @param targetPos The target position
*/
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -86,7 +86,7 @@ public class DrivePositionMPAux extends CommandBase {
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
return true;
//return true;
}
return false;
}
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+2 -2
View File
@@ -38,8 +38,8 @@ 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 steerInput = m_controller.getRightXAxis();
double moveInput = m_controller.getRightXAxis();
double steerInput = -m_controller.getLeftYAxis();
double moveOutput = 0;
double steerOutput = 0;
if (moveInput >= 0){
src/main/java/frc4388/robot/commands/TurnDegrees.java
+1 -1
View File
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
return true;
}
return false;
src/main/java/frc4388/robot/subsystems/Drive.java
+17 -17
View File
@@ -106,13 +106,6 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
/* Config Open Loop Ramp so we don't make sudden output changes */
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -120,10 +113,10 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
/* Config Supply Current Limit (Use only for debugging) */
m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
//m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
/* Config deadbands so that */
m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -252,6 +245,13 @@ public class Drive extends SubsystemBase {
/* Set up Orchestra */
m_orchestra = new Orchestra();
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
//m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
/* Set up music for drive train */
m_orchestra.addInstrument(m_leftBackMotor);
@@ -322,7 +322,7 @@ public class Drive extends SubsystemBase {
* using the Differential Drive class to manage the two inputs
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(steer, move);
m_driveTrain.arcadeDrive(move, steer);
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
}
@@ -723,8 +723,8 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
//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());
@@ -756,9 +756,9 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
//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 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 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));
@@ -766,7 +766,7 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
if (m_currentSong != m_songChooser.getSelected()){
m_currentSong = m_songChooser.getSelected();