Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-08 16:28:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

All low gear PIDs WORKY, test auto command WORKY

Browse Source
This commit is contained in:
Aarav Shah
2020-02-28 17:38:15 -07:00
parent 08b8797152
commit c572182678
6 changed files with 26 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -55,8 +55,8 @@ public final class Constants {
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.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;
public static final int DRIVE_CRUISE_VELOCITY = 25000;
public static final int DRIVE_ACCELERATION = 21000;
public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
src/main/java/frc4388/robot/RobotContainer.java
+18 -16
View File
@@ -30,6 +30,8 @@ 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.DriveStraightToPositionMM;
import frc4388.robot.commands.DriveStraightToPositionPID;
import frc4388.robot.commands.DriveWithJoystick;
import frc4388.robot.commands.RunClimberWithTriggers;
import frc4388.robot.commands.RunExtenderOutIn;
@@ -114,19 +116,19 @@ public class RobotContainer {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new TurnDegrees(90, m_robotDrive));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
.whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0));
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
.whenPressed(new InstantCommand());
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
/* Driver Buttons */
// sets solenoids into high gear
@@ -192,25 +194,25 @@ public class RobotContainer {
return new SequentialCommandGroup(new Wait(2, m_robotDrive),
//add aim command
//add shooter command
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
new ParallelCommandGroup(
new DriveStraightToPositionMM(m_robotDrive, 48.0),
/*new ParallelCommandGroup(
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
new ParallelCommandGroup(
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/
//add aim command
//add shooter command
//Below this would be the picking up additional balls outside of those in the trench directly behind us
new TurnDegrees(-150, m_robotDrive),
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
new TurnDegrees(75, m_robotDrive),
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
new TurnDegrees(-45, m_robotDrive),
new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
new TurnDegrees(m_robotDrive, -90),
new DriveStraightToPositionMM(m_robotDrive, 96.0));//,
//new StorageIntakeGroup(m_robotIntake, m_robotStorage),
//new TurnDegrees(m_robotDrive, 75),
//new DriveStraightToPositionMM(m_robotDrive, 18.0),
//new TurnDegrees(m_robotDrive, -45),
//new DriveStraightToPositionMM(m_robotDrive, 12.0));
}
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(
src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+3 -3
View File
@@ -66,13 +66,13 @@ public class DrivePositionMPAux extends CommandBase {
if (System.currentTimeMillis() - m_startTime < m_rampRate) {
// Ramping
m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
} else if (m_targetPos - m_currentPos > m_rampDist) {
// Cruising
m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
} else {
// Deramp PID
m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
}
m_counter ++;
}
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+1 -1
View File
@@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
return true;
} else {
i++;
src/main/java/frc4388/robot/commands/TurnDegrees.java
+1 -1
View File
@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
/**
* Creates a new TurnDeg.
*/
public TurnDegrees(double targetAngle, Drive subsystem) {
public TurnDegrees(Drive subsystem, double targetAngle) {
// Use addRequirements() here to declare subsystem dependencies.
m_targetAngle = targetAngle;