Skip to content

Commit

Permalink
some more semi auto
Browse files Browse the repository at this point in the history
  • Loading branch information
@anorakthegreat
anorakthegreat committed Jan 29, 2025
1 parent 2b0f011 commit 829b272
Show file tree
Hide file tree
Showing 32 changed files with 2,085 additions and 55 deletions.
155 changes: 141 additions & 14 deletions comp/src/main/java/org/team100/frc2024/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,28 @@
/** Add your docs here. */
public class FieldConstants {
public enum FieldSector {
S1,
S2,
S3,
S4,
S5,
S6
AB,
CD,
EF,
GH,
IJ,
KL
}

public enum ReefPoint {
A,
B,
C,
D,
E,
F,
G,
H,
I,
J,
K,
L,
CENTER
}

public static FieldConstants.FieldSector getSector(Pose2d pose){
Expand All @@ -27,27 +43,138 @@ public static FieldConstants.FieldSector getSector(Pose2d pose){
System.out.println("ANGLEEEE" + angle);

if(angle >= -30 && angle <= 30){
return FieldSector.S1;
return FieldSector.AB;
} else if(angle >= -90 && angle <-30){
return FieldSector.S2;
return FieldSector.KL;
} else if(angle >= -150 && angle <= -90){
return FieldSector.S3;
return FieldSector.IJ;
} else if(angle >= 150 || angle <= -150){
return FieldSector.S4;
return FieldSector.GH;
} else if(angle >= 90 && angle <= 150){
return FieldSector.S5;
return FieldSector.EF;
} else if(angle >= 30 && angle <= 90){
return FieldSector.S6;
return FieldSector.CD;
} else {
return FieldSector.S1;
return FieldSector.AB;
}
}

public static Rotation2d getSectorAngle(FieldSector sector){
switch(sector){
case AB:
return Rotation2d.fromDegrees(180);
case CD:
return Rotation2d.fromDegrees(-120);
case EF:
return Rotation2d.fromDegrees(-60);
case GH:
return Rotation2d.fromDegrees(0);
case IJ:
return Rotation2d.fromDegrees(60);
case KL:
return Rotation2d.fromDegrees(120);
default:
return Rotation2d.fromDegrees(0);
}
}

public static Rotation2d angleToReefCenter(Pose2d pose){
Translation2d target = FieldConstants.getReefCenter().minus(pose.getTranslation());
Rotation2d targetAngle = target.getAngle();
return targetAngle;
}

public static Translation2d getReefCenter(){ //blue
return new Translation2d(4.508405, 4.038690);
}

public static double getOrbitRadius(){
public static double getOrbitWaypointRadius(){
return 2.7;
}

public static double getOrbitDestinationRadius(){
return 1.6;
}

public static double getReefOffset(){
return 0.5;
}

public static Translation2d getOrbitWaypoint(Rotation2d angle){
Translation2d reefCenter = getReefCenter();
double x = reefCenter.getX() + getOrbitWaypointRadius() * angle.getCos();
double y = reefCenter.getY() + getOrbitWaypointRadius() * angle.getSin();

return new Translation2d(x, y);

}

public static Translation2d getOrbitWaypoint(FieldSector sector){
Translation2d reefCenter = getReefCenter();
double x = reefCenter.getX() + getOrbitWaypointRadius() * getSectorAngle(sector).getCos();
double y = reefCenter.getY() + getOrbitWaypointRadius() * getSectorAngle(sector).getSin();

return new Translation2d(x, y);

}

public static Translation2d getOrbitDestination(FieldSector destinationSector, ReefPoint destinationPoint){
Translation2d reefCenter = getReefCenter();
Rotation2d sectorAngle = getSectorAngle(destinationSector);



double x = reefCenter.getX() + getOrbitDestinationRadius() * sectorAngle.getCos();
double y = reefCenter.getY() + getOrbitDestinationRadius() * sectorAngle.getSin();

double angleTheta = Math.atan(getReefOffset()/getOrbitDestinationRadius());
double newRadius = Math.sqrt(Math.pow(getReefOffset(), 2) + Math.pow(getOrbitDestinationRadius(), 2));



switch(destinationPoint){
case A, C, E, G, I, K:
sectorAngle = sectorAngle.plus(Rotation2d.fromRadians(angleTheta));
break;
case B, D, F, H, J, L:
sectorAngle = sectorAngle.minus(Rotation2d.fromRadians(angleTheta));
break;
case CENTER:
break;
}


switch (destinationSector){
case AB:
sectorAngle = Rotation2d.fromDegrees(180).minus(sectorAngle);
break;
case CD:
sectorAngle = Rotation2d.fromDegrees(-120).minus(sectorAngle);
break;
case EF:
sectorAngle = Rotation2d.fromDegrees(-60).minus(sectorAngle);
break;
case GH:
sectorAngle = Rotation2d.fromDegrees(0).minus(sectorAngle);
break;
case IJ:
sectorAngle = Rotation2d.fromDegrees(60).minus(sectorAngle);
break;
case KL:
sectorAngle = Rotation2d.fromDegrees(120).minus(sectorAngle);
break;
}
Rotation2d angleToCenter = Rotation2d.fromDegrees(90).minus(sectorAngle);
if(angleToCenter.getDegrees() < 0){
angleToCenter = new Rotation2d(-angleToCenter.getDegrees());
}



x = newRadius * angleToCenter.getCos();
y = newRadius * angleToCenter.getSin();

return new Translation2d(x, y);

}
}
31 changes: 24 additions & 7 deletions comp/src/main/java/org/team100/frc2024/Swerve/Maker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,13 @@
import java.util.function.Supplier;

import org.team100.frc2024.FieldConstants;
import org.team100.frc2024.Swerve.SemiAuto.DriveTo_GH;
import org.team100.frc2024.Swerve.SemiAuto.Planner2025;
import org.team100.frc2024.Swerve.SemiAuto.DriveTo_IJ;
import org.team100.frc2024.Swerve.SemiAuto.DriveTo_KL;
import org.team100.lib.commands.drivetrain.TrajectoryCommand100;
import org.team100.lib.follower.DrivePIDFFollower;
import org.team100.lib.follower.DrivePIDFLockFollower;
import org.team100.lib.follower.DriveTrajectoryFollower;
import org.team100.lib.follower.DriveTrajectoryFollowerFactory;
import org.team100.lib.logging.LoggerFactory;
Expand All @@ -25,14 +30,21 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;


public class Maker {

TimingConstraintFactory constraints;
DrivePIDFFollower.Log m_PIDFLog;
DrivePIDFLockFollower.Log m_lockedLog;

LoggerFactory m_logger;
DriveToS4.Log m_commandLog;
DriveTo_GH.Log m_driveToGHLog;
DriveTo_IJ.Log m_driveToIJLog;
DriveTo_KL.Log m_driveToKLLog;


SwerveDriveSubsystem m_swerve;
DriveTrajectoryFollowerFactory m_factory;
TrajectoryVisualization m_viz;
Expand All @@ -43,7 +55,12 @@ public class Maker {
public Maker(LoggerFactory parent, SwerveDriveSubsystem swerve, DriveTrajectoryFollowerFactory factory, SwerveKinodynamics kinodynamics, TrajectoryVisualization viz){
m_logger = parent.child("Maker");
m_PIDFLog = new DrivePIDFFollower.Log(m_logger);
m_commandLog = new DriveToS4.Log(m_logger);
m_lockedLog = new DrivePIDFLockFollower.Log(m_logger);

m_driveToGHLog = new DriveTo_GH.Log(m_logger);
m_driveToIJLog = new DriveTo_IJ.Log(m_logger);
m_driveToKLLog = new DriveTo_KL.Log(m_logger);

m_factory = factory;
m_swerve = swerve;
constraints = new TimingConstraintFactory(kinodynamics);
Expand All @@ -63,7 +80,7 @@ public Pose2d makeTrajectoryCommand(Supplier<Pose2d> robotPose) {
// Optional<TrajectorySamplePoint> futurePoint = iter.advance(0.4);

double[] center = { FieldConstants.getReefCenter().getX(), FieldConstants.getReefCenter().getY()};
double radius = FieldConstants.getOrbitRadius();
double radius = FieldConstants.getOrbitWaypointRadius();
double[] point = {robotPose.get().getX(), robotPose.get().getY()};

List<double[]> array = getTangentPointsAtCircle(FieldConstants.getReefCenter().getX(), FieldConstants.getReefCenter().getY(), radius, robotPose.get().getX(), robotPose.get().getY());
Expand All @@ -81,7 +98,7 @@ public Pose2d makeTrajectoryCommand(Supplier<Pose2d> robotPose) {
public void makeClockWiseTraj() {

Translation2d reefCenter = FieldConstants.getReefCenter();
double circleRadius = FieldConstants.getOrbitRadius();
double circleRadius = FieldConstants.getOrbitWaypointRadius();

Pose2d radian_0 = new Pose2d(reefCenter.getX() + circleRadius, reefCenter.getY(), Rotation2d.fromDegrees(270));
Pose2d radian_90 = new Pose2d(reefCenter.getX(), reefCenter.getY() - circleRadius, Rotation2d.fromDegrees(180));
Expand Down Expand Up @@ -115,7 +132,7 @@ public void makeClockWiseTraj() {

}

public DriveToS4 test() {
public Command test() {

Pose2d waypoint1 = new Pose2d(5.37, 6, Rotation2d.fromDegrees(315));
Pose2d waypoint2 = new Pose2d(6.305274, 4.074270, Rotation2d.fromDegrees(270));
Expand All @@ -130,8 +147,8 @@ public DriveToS4 test() {
new Rotation2d()
));

return new DriveToS4(
m_commandLog,
return new DriveTo_IJ(
m_driveToIJLog,
m_swerve,
waypointsM,
headings,
Expand Down
Loading

0 comments on commit 829b272

Please sign in to comment.