Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Emcc #15

Merged
merged 4 commits into from
Nov 17, 2023
Merged

Emcc #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4d877e6
added place auto
Speedy6451 Nov 4, 2023
1e3a1aa
added SLOW mode
Speedy6451 Nov 4, 2023
96d331b
deadzone
Speedy6451 Nov 4, 2023
8770574
extract deadzone to utils
Speedy6451 Nov 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions src/main/java/com/team2502/robot2023/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -355,6 +355,10 @@ public static final class Drivetrain {
public static final double MAX_ROT = 3; // driver rotation gain (rad/s) 9 - 4
public static final double RET_VEL = 1; // driver speed gain (m/s)
public static final double RET_ROT = 0.25; // driver rotation gain (rad/s)
public static final double OI_DEADZONE_XY = 0.02; // Joystick deadzone
public static final double OI_DEADZONE_Z = 0.03; // Joystick twist deadzone
public static final double DEADZONE_XY = 0.05; // Transpose deadzone
public static final double DEADZONE_THETA = 0.05; // Rotation deadzone

// constants for pose control
public static final double DRIVETRAIN_MOVE_P = 2.7; // 0.6
Expand Down
40 changes: 40 additions & 0 deletions src/main/java/com/team2502/robot2023/Utils.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,4 +103,44 @@ public static double findDist(double camHeight, double camElevation, double bask
Math.tan(Math.toRadians(targetElevation+ camElevation))
);
}

/**
* map x from one deadzone to another
* @param minInput start of input deadzone
* @param minOutput start of mapped deadzone
* @param x value to map
* @param scale gain
* @return transformed x
*/
public static double deadzone(double minInput, double minOutput, double x, double scale) {
boolean sign = x > 0.0;
x = sign ? x : -x;
double inputRange = 1.0 - minInput;
double outpuRange = 1.0 - minOutput;

// below input deadzone return 0
if (x < minInput) {
return 0;
}

// this is some kind of affine transformation
double inputAdjusted = (x-minInput) * inputRange;

inputAdjusted *= scale;

double outputAjusted = inputAdjusted / outpuRange + minOutput;

return sign ? outputAjusted : -outputAjusted;
}

/**
* map x from one deadzone to another
* @param minInput start of input deadzone
* @param minOutput start of mapped deadzone
* @param x value to map
* @return transformed x
*/
public static double deadzone(double minInput, double minOutput, double x) {
return deadzone(minInput, minOutput, x, 1.0);
}
}
12 changes: 12 additions & 0 deletions src/main/java/com/team2502/robot2023/autonomous/Autos.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,18 @@ public enum Autos { // first auto is default
new FollowPathAbsoluteCommand(d, "testpath")
)),

PLACE_CUBE("P", (d,i,e) -> Commands.sequence(
new InstantCommand(d::resetPitch),
new InstantCommand(d::resetHeading),
new InstantCommand(() -> d.setPose(new Pose2d(1.75, 4.45, Rotation2d.fromDegrees(0))), d),
new InstantCommand(() -> i.setSpeed(0.2)),
Commands.deadline(Commands.waitSeconds(2.25), new SetArmSimpleCommand(e, ElevatorPosition.CUBE_TOP, IntakePosition.CUBE_TOP)),
Commands.deadline(Commands.waitSeconds(0.125), new InstantCommand(() -> i.setSpeed(-0.25))),
Commands.deadline(new InstantCommand(() -> i.setSpeed(0))),
Commands.deadline(Commands.waitSeconds(1.5), new SetArmSimpleCommand(e, ElevatorPosition.BOTTOM, IntakePosition.CUBE_GROUND)),
Commands.deadline(new InstantCommand(() -> i.setSpeed(0.5)))
)),

PLACE_CUBE_GRAB_CUBE("PP left", (d,i,e) -> Commands.sequence(
new InstantCommand(d::resetPitch),
new InstantCommand(d::resetHeading),
Expand Down
37 changes: 30 additions & 7 deletions src/main/java/com/team2502/robot2023/commands/DriveCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import com.team2502.robot2023.Constants.OI;
import com.team2502.robot2023.Constants.Subsystems.Drivetrain;
import com.team2502.robot2023.subsystems.DrivetrainSubsystem;
import com.team2502.robot2023.Utils;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
Expand All @@ -31,6 +32,8 @@ private enum Drivetype {
FieldOriented,
RobotOriented,
FieldOrientedTwist,
FieldOrientedTwistRet,
FieldOrientedTwistRetDead,
FieldOrientedTwistPow,
RobotOrientedCenteredRot,
VirtualTank,
Expand All @@ -40,7 +43,7 @@ private enum Drivetype {
private double dlxDrift;
private double dlyDrift;

private boolean tgoggla = false;
private boolean slowmode = false;
private boolean prevTog = false;

private final SendableChooser<Drivetype> typeEntry = new SendableChooser<>();
Expand All @@ -58,7 +61,9 @@ public DriveCommand(DrivetrainSubsystem drivetrain, Joystick joystickDriveLeft,
typeEntry.addOption("nolan mode", Drivetype.RobotOrientedCenteredRot);
typeEntry.addOption("Robot Oriented", Drivetype.RobotOriented);
typeEntry.addOption("Field Pow", Drivetype.FieldOrientedTwistPow);
typeEntry.setDefaultOption("Field Twist", Drivetype.FieldOrientedTwist);
typeEntry.addOption("Field Twist", Drivetype.FieldOrientedTwist);
typeEntry.addOption("Super Loud Olympic Winning", Drivetype.FieldOrientedTwistRet);
typeEntry.setDefaultOption("Dead", Drivetype.FieldOrientedTwistRetDead);
SmartDashboard.putData("Drive Type", typeEntry);

controllerEntry.addOption("Joystick", DriveController.Joystick);
Expand All @@ -76,11 +81,11 @@ public void execute() {

boolean tog = leftJoystick.getRawButton(OI.RET_MODE);
if (prevTog != tog && tog) {
tgoggla = !tgoggla;
slowmode = !slowmode;
}
prevTog = tog;

SmartDashboard.putBoolean("iosajioj", tgoggla);
SmartDashboard.putBoolean("iosajioj", slowmode);

ChassisSpeeds speeds;
Translation2d centerOfRotation;
Expand Down Expand Up @@ -117,9 +122,27 @@ public void execute() {
break;
case FieldOrientedTwist:
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
leftJoystick.getY() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-leftJoystick.getX() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-rightJoystick.getZ() * (tgoggla ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
leftJoystick.getY() * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-leftJoystick.getX() * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-rightJoystick.getZ() * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset));
centerOfRotation = new Translation2d(0, 0);
drivetrain.setSpeeds(speeds, centerOfRotation);
break;
case FieldOrientedTwistRet:
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.08, leftJoystick.getY()) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL) * 0.65,
-Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.08, leftJoystick.getX()) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL) * 0.65,
-Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.07, rightJoystick.getZ()) * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT) * 0.65,
Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset));
centerOfRotation = new Translation2d(0, 0);
drivetrain.setSpeeds(speeds, centerOfRotation);
break;
case FieldOrientedTwistRetDead:
speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getY(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getX(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.04, rightJoystick.getZ(), 0.55 * 0.6) * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset));
centerOfRotation = new Translation2d(0, 0);
drivetrain.setSpeeds(speeds, centerOfRotation);
Expand Down