Skip to content

Commit

Permalink
Merge pull request #15 from Team-2502/emcc
Browse files Browse the repository at this point in the history 
Emcc
  • Loading branch information
@Malevolence5
Malevolence5 authored Nov 17, 2023
2 parents 4106e23 + 8770574 commit ff114d1
Show file tree
Hide file tree
Showing 4 changed files with 86 additions and 7 deletions.
4 changes: 4 additions & 0 deletions src/main/java/com/team2502/robot2023/Constants.java
View file
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
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
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
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

0 comments on commit ff114d1

Please sign in to comment.