Skip to content

Commit

Permalink
Competition Code (#30)
Browse files Browse the repository at this point in the history 
* Integrated math, need to get constants

* WIP

* fix wip

* fix: passes inspection (barely)

* WIP

* feat: charged station auton 6 sec

* feat: drops cube

* tweak max speed

* feat: moves arm

* chore: formatting

* fix: BalanceCommand in autonomous

* fix: overide button

---------

Co-authored-by: ProgrammingSR <[email protected]>
  • Loading branch information
@ReeledWarrior14 @ProgrammingSR
ReeledWarrior14 and ProgrammingSR authored Mar 7, 2023
1 parent 162005f commit e347487
Show file tree
Hide file tree
Showing 7 changed files with 177 additions and 42 deletions.
5 changes: 0 additions & 5 deletions simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,7 @@
},
"windows": {
"/SmartDashboard/Field": {
"bottom": 544,
"height": 8.013679504394531,
"image": "C:\\Users\\Public\\Documents\\vscode-workspace\\ChargedUp2023\\.pathplanner\\2023-field.png",
"left": 46,
"right": 1088,
"top": 36,
"width": 16.541748046875,
"window": {
"visible": true
Expand Down
15 changes: 13 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,9 @@ public static final class ModuleConstants {
}

public static final class ArmConstants {
public static final double floor2ArmBase = 10f / 39.37;
public static final double detector2tip = 42f / 39.37;

public static final int kPivotMotorPort = 36;
public static final int kElevatorMotorPort = 34;
public static final int kPivotEncoderPort = 7;
Expand All @@ -107,10 +110,18 @@ public static final class ArmConstants {
public static final double gearRatio = 4 / 1;

public static final double kMinSwitchPos = 0;
public static final double kMaxSwitchPos = 0.61595;
public static final double kMaxSwitchPos = 0.42; //0.61595;

public static final double kMinPivotPos = 0; // TODO find accurate min and max for pivot
public static final double kMinPivotPos = 0;
public static final double kMaxPivotPos = 90; // 0 is straight up

public static final double kPivotAxleHeight = 0.2286; // All values in meters
public static final double kAxleToFrontPerimeter = 0.5588;
public static final double kMaxExtensionHeight = 1.9812 - 0.05; // minus 5 centimeters of leeway
public static final double kMaxFrameExtensionLimit = 1.2192 - 0.02; // minus 2 centimeters of leeway
public static final double kElevatorMaxExtensionOffset = 0.3683; // L offset
public static final double kGrabberToDetector = 40.5/39.3;
public static final double kLimitToAxle = 0.149678698451591;
}

public static final class GrabberConstants {
Expand Down
68 changes: 46 additions & 22 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@

import java.util.HashMap;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.auto.PIDConstants;
import com.pathplanner.lib.auto.SwerveAutoBuilder;

Expand All @@ -18,12 +16,15 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants.ArmConstants;
import frc.robot.Constants.AutonConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.commands.AutonDriveCommand;
import frc.robot.commands.BalanceCommand;
import frc.robot.commands.SnapRotateCommand;
import frc.robot.subsystems.ArmSubsystem;
Expand All @@ -43,7 +44,6 @@ public class RobotContainer {

private final XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
private final XboxController m_operatorController = new XboxController(OIConstants.kOperatorControllerPort);

private final SendableChooser<String> m_chooser = new SendableChooser<>();
private final HashMap<String, Command> m_eventMap = new HashMap<>();
private final SwerveAutoBuilder m_autoBuilder = new SwerveAutoBuilder(
Expand Down Expand Up @@ -73,19 +73,23 @@ public RobotContainer() {
-m_driverController.getLeftY(),
OIConstants.kControllerDeadband)
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController.getLeftTriggerAxis() * OIConstants.kSlowModeScalar)
/ 3,
* (1 - m_driverController
.getLeftTriggerAxis()
* OIConstants.kSlowModeScalar)
/ 2,
MathUtil.applyDeadband(
-m_driverController.getLeftX(),
OIConstants.kControllerDeadband)
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController.getLeftTriggerAxis() * OIConstants.kSlowModeScalar)
/ 3,
* (1 - m_driverController
.getLeftTriggerAxis()
* OIConstants.kSlowModeScalar)
/ 2,
MathUtil.applyDeadband(
-m_driverController.getRightX(),
OIConstants.kControllerDeadband)
* DriveConstants.kMaxAngularSpeedRadiansPerSecond
/ 3,
/ 2,
!m_driverController.getRightBumper()),
m_robotDrive));

Expand Down Expand Up @@ -143,6 +147,10 @@ private void configureButtonBindings() {
.toggleOnTrue(new InstantCommand(armSubsystem::togglePID, armSubsystem))
.toggleOnFalse(new InstantCommand(armSubsystem::togglePID, armSubsystem));

new JoystickButton(m_operatorController, Button.kY.value)
.whileTrue(new InstantCommand(armSubsystem::forceForwards, armSubsystem))
.onFalse(new InstantCommand(armSubsystem::stopForce, armSubsystem));

/*
* DO NOT USE: ROBOT WILL BREAK
* new POVButton(m_operatorController, 0)
Expand Down Expand Up @@ -172,18 +180,34 @@ private void configureButtonBindings() {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
String path;
if (m_chooser.getSelected() != null) {
path = m_chooser.getSelected();
} else {
return null;
}

return m_autoBuilder.fullAuto(
PathPlanner.loadPathGroup(
path,
new PathConstraints(
AutonConstants.maxVelocity,
AutonConstants.maxAcceleration)));
// Timer m_autonTimer = new Timer();
// m_autonTimer.start();

// String path;
// if (m_chooser.getSelected() != null) {
// path = m_chooser.getSelected();
// } else {
// return null;
// }

// return m_autoBuilder.fullAuto(
// PathPlanner.loadPathGroup(
// path,
// new PathConstraints(
// AutonConstants.maxVelocity,
// AutonConstants.maxAcceleration)));

return new SequentialCommandGroup(
new ParallelDeadlineGroup(
new WaitCommand(1),
new RunCommand(() -> armSubsystem.setArmPos(20, 0.1), armSubsystem)),
new ParallelDeadlineGroup(
new WaitCommand(2),
new RunCommand(() -> armSubsystem.setArmPos(45, 0.3), armSubsystem)),
new InstantCommand(grabberSubsystem::toggle, grabberSubsystem),
new ParallelDeadlineGroup(
new WaitCommand(6),
new AutonDriveCommand(m_robotDrive)),
new BalanceCommand(m_robotDrive));
}
}
20 changes: 20 additions & 0 deletions src/main/java/frc/robot/commands/AutonDriveCommand.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DriveSubsystem;

public class AutonDriveCommand extends CommandBase {
DriveSubsystem m_driveSubsystem;

public AutonDriveCommand(DriveSubsystem drive) {
m_driveSubsystem = drive;
addRequirements(m_driveSubsystem);

}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_driveSubsystem.drive(-0.5, 0, 0, false);
}
}
12 changes: 7 additions & 5 deletions src/main/java/frc/robot/commands/BalanceCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,15 @@

package frc.robot.commands;

import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants.DriveConstants;
import frc.robot.subsystems.DriveSubsystem;

/** Uses a PID and the gyroscope to balance the robot on the charger. */
public class BalanceCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private final BangBangController m_bangBang = new BangBangController(DriveConstants.kToleranceBalance);
private final PIDController m_PID = new PIDController(0.025, 0, 0);

/**
* Creates a new {@link BalanceCommand}.
Expand All @@ -22,15 +22,17 @@ public class BalanceCommand extends CommandBase {
public BalanceCommand(DriveSubsystem subsystem) {
m_subsystem = subsystem;
addRequirements(m_subsystem);

m_PID.setTolerance(DriveConstants.kToleranceBalance);
}

@Override
public void execute() {
m_subsystem.drive(
m_bangBang.calculate(m_subsystem.getGyroPitch(), 0) * 0.2,
m_PID.calculate(m_subsystem.getGyroPitch(), 0),
0,
0,
true);
false);
}

@Override
Expand All @@ -40,6 +42,6 @@ public void end(boolean interrupted) {

@Override
public boolean isFinished() {
return m_bangBang.atSetpoint();
return false;
}
}
96 changes: 90 additions & 6 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

public class ArmSubsystem extends SubsystemBase {
private final CANSparkMax m_pivotMotor = new CANSparkMax(ArmConstants.kPivotMotorPort,
MotorType.kBrushless); // TODO: TEST IF PIVOT MOTOR NEEDS TO BE INVERTED
MotorType.kBrushless);
private final CANSparkMax m_elevatorMotor = new CANSparkMax(ArmConstants.kElevatorMotorPort,
MotorType.kBrushless);

Expand All @@ -38,7 +38,6 @@ public class ArmSubsystem extends SubsystemBase {

public boolean seenSwitch = false; // Elevator won't move down until we see the switch

//TODO: measure this later
private double m_encoderElevatorOffset = -0.149678698451591; // Encoder offset, updtated whenever a limit switch goes LOW, in meters

// Keeps track of whether we want to allow pivot to raise. This is used to stop the terrible oscilation when the pivot fully retracts (because the spring keeps pushing the arm away from the setpoint
Expand All @@ -47,6 +46,8 @@ public class ArmSubsystem extends SubsystemBase {
//Keeps track if we want to stop PIDs
private boolean stopPID = false;

private boolean forcing = false;

/**
* Constructs a {@link ArmSubsystem}.
* Does not move arm above limit switch automatically and always moves arm to
Expand Down Expand Up @@ -80,10 +81,27 @@ public void enable() {
stopPID = false;
}

public void forceForwards(){
m_pivotMotor.set(0.2);
SmartDashboard.putString("turning", "tt");
forcing = true;
}

public void stopForce(){
m_pivotMotor.set(0);
SmartDashboard.putString("turning", "ttn");
forcing = false;
}

@Override
public void periodic() {
double ps = m_pivotPID.calculate(m_pivotEncoder.getAbsolutePosition());
double es = m_elevatorPID.calculate(getElevatorEncoder());

SmartDashboard.putNumber("Elevator Encoder", getElevatorEncoder()); //m
// SmartDashboard.putNumber("elevator distance difference (length until max)", getElevatorMax() - getElevatorEncoder() * 39.3);
SmartDashboard.putNumber("Pivot Encoder", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putBoolean("seenSwitch", seenSwitch);
/*
//TODO: remove local variable and some smart dashboard outputs
Expand Down Expand Up @@ -130,6 +148,10 @@ public void periodic() {
return;
}

// if (seenSwitch) { // TODO: TEST instead of the 2 if statements below
// m_elevatorPID.setSetpoint(MathUtil.clamp(m_elevatorPID.getSetpoint(), ArmConstants.kMinSwitchPos + 0.075, ArmConstants.kMaxSwitchPos - 0.01));
// }

//Check for underextension (physical), if so increase the setpoint
if (m_elevatorPID.getSetpoint() < (ArmConstants.kMinSwitchPos + 0.075) && seenSwitch) {
m_elevatorPID.setSetpoint(ArmConstants.kMinSwitchPos + 0.075);
Expand All @@ -152,9 +174,21 @@ public void periodic() {
return;
}

// SmartDashboard.putNumber("elevator distance difference (length until max)", (getElevatorMaxCheckpoints() - getElevatorEncoder()) * 39.3);
// SmartDashboard.putNumber("elevator", getElevatorEncoder());
// SmartDashboard.putNumber("max ext", getElevatorMaxCheckpoints());
// SmartDashboard.putBoolean("elevatorMaxCheck", m_elevatorPID.getSetpoint() > getElevatorMaxCheckpoints());
// if (m_elevatorPID.getSetpoint() > getElevatorMaxCheckpoints()) {
// m_elevatorMotor.set(0);
// m_elevatorPID.setSetpoint(getElevatorEncoder());
// SmartDashboard.putNumber("elevatorMaxSetpoint", m_elevatorPID.getSetpoint());

// return;
// }

//Stop pivoting backwards
if (m_pivotPID.getSetpoint() < ArmConstants.kMinPivotPos + 3) {
m_pivotMotor.set(0);
if (!forcing) m_pivotMotor.set(0);

//Stops oscilation (due to spring and low gravitational influence on torque)
plock = true;
Expand All @@ -178,13 +212,14 @@ public void periodic() {
//TODO: add anti oscilation with POV setpoints (if any setpoint may cause oscilation, current anti-oscilation code via plock only works with manual control)
if (seenSwitch || ps < 0 || m_pivotPID.getSetpoint() <= 20) {
if (!stopPID)
m_pivotMotor.set(MathUtil.clamp(ps, -0.3, 0.3)); // TODO: adjust clamp value
if (!forcing)
m_pivotMotor.set(MathUtil.clamp(ps, -0.3, 0.3));
}

if (!stopPID) {
double motorSpeed = MathUtil.clamp(es, -0.25, 0.25)
+ (0.015 * Math.cos(m_pivotEncoder.getPosition()));
m_elevatorMotor.set(motorSpeed); // TODO: take angle into account
+ (0.015 * Math.cos(m_pivotEncoder.getAbsolutePosition()));
m_elevatorMotor.set(motorSpeed);
SmartDashboard.putNumber("motorSpeed", motorSpeed);
} else {
SmartDashboard.putNumber("motorSpeed", 999);
Expand Down Expand Up @@ -260,6 +295,11 @@ public void setArmSpeeds(double pivotSpeed, double elevatorSpeed) {
m_elevatorPID.setSetpoint(m_elevatorPID.getSetpoint() + (elevatorSpeed * Robot.kDefaultPeriod));
}

public void setArmPos(double pivotA, double elevatorP){
m_elevatorPID.setSetpoint(elevatorP);
m_pivotPID.setSetpoint(pivotA);
}

/**
* Converts elevator spark max encoder value to meters
*
Expand Down Expand Up @@ -287,4 +327,48 @@ private double getElevatorEncoder() {
// should setPositionConversionFactor on the motor rather than using
// encoderToMeters
}

// private double getElevatorMaxCheckpoints () {
// double pivotEncoderVal = MathUtil.clamp(m_pivotEncoder.getAbsolutePosition(), 0, 90);
// if (pivotEncoderVal >= 0 && pivotEncoderVal <= ArmConstants.kTopPivot) {
// return 0;
// }
// if (pivotEncoderVal <= ArmConstants.kMidPivot)
// }

private double getElevatorMaxCheckpoints () {
double pivotEncoderVal = Math.toRadians(MathUtil.clamp(m_pivotEncoder.getAbsolutePosition(), 0, 90));

double max_extension = (ArmConstants.kMaxFrameExtensionLimit + ArmConstants.kAxleToFrontPerimeter) / (pivotEncoderVal == 0 ? 1.0e-10 : Math.sin(pivotEncoderVal));
max_extension -= ArmConstants.detector2tip + ArmConstants.kLimitToAxle;

double max_height = (ArmConstants.kMaxExtensionHeight) / (pivotEncoderVal == Math.PI/2 ? 1.0e-10 : Math.cos(pivotEncoderVal));
max_height -= ArmConstants.floor2ArmBase + ArmConstants.detector2tip;

return Math.min(max_height, max_extension);
}

private double getElevatorMax() {
double maxHeight = 999;
double maxExtension = 999;
double pivotEncoderValue = MathUtil.clamp(m_pivotEncoder.getAbsolutePosition(), 0, 90); // Ensures that the pivot encoder does not update between calls

// grabberToDetector = value between the limit switch detector and the tip of the grabber
// extensionLimit = horizontal distance between limit switch and the front of frame



if (pivotEncoderValue != 0) {
maxExtension = (ArmConstants.kMaxFrameExtensionLimit + ArmConstants.kAxleToFrontPerimeter)/Math.sin(Math.toRadians(pivotEncoderValue));
}
if (pivotEncoderValue != 90) {
maxHeight = (ArmConstants.kMaxExtensionHeight - ArmConstants.kPivotAxleHeight)/Math.cos(Math.toRadians(pivotEncoderValue));
}

SmartDashboard.putNumber("maxHeight", maxHeight * 39.3);
SmartDashboard.putNumber("maxExtension", maxExtension * 39.3);

return Math.min(maxHeight, maxExtension) - ArmConstants.kElevatorMaxExtensionOffset; // TODO: Check math with Greg
}
}

Loading

0 comments on commit e347487

Please sign in to comment.