Skip to content

Commit

Permalink
:D
Browse files Browse the repository at this point in the history
  • Loading branch information
IsaacThoman committed Apr 24, 2024
1 parent 1604045 commit c470450
Showing 1 changed file with 28 additions and 3 deletions.
31 changes: 28 additions & 3 deletions src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -131,14 +131,39 @@ public SwerveModulePosition[] getPositions(){
return new SwerveModulePosition[]{frontLeft.getPosition(),frontRight.getPosition(),rearLeft.getPosition(),rearRight.getPosition()};
}


boolean personLastSeenOnLeft = false;
public void aimAtPerson() {


double[] personXList = personTable.getEntry("personList").getDoubleArray(new double[]{});

if (personTable.getEntry("personCount").getInteger(0) == 0) {
personX = 0;
SmartDashboard.putNumberArray("personXListReceived",personXList);

if(personXList.length<1) {
// drive(0,0,0);
SmartDashboard.putNumber("personX",5);

if (personLastSeenOnLeft) {
drive(0, 0, 0.3);
} else {
drive(0, 0, -0.3);
}

return;
}

double personX = personXList[0];

SmartDashboard.putNumber("personX", personX);

for(int i = 1; i <personXList.length; i++)
if(Math.abs(personX) > Math.abs(personXList[i]))
personX = personXList[i];

if (personX > 0) {
personLastSeenOnLeft = false;
} else {
personLastSeenOnLeft = true;
}

personX *= 4;
Expand Down

0 comments on commit c470450

Please sign in to comment.