Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

some aimtocenter stuff, unfinished + has errors

Browse Source
This commit is contained in:
evan
2022-02-24 18:12:57 -07:00
parent f97c7d8b9d
commit 449a3dc8c4
4 changed files with 187 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+14 -12
View File
@@ -17,7 +17,7 @@ public class AimToCenter extends CommandBase {
double x = 0;
double y = 0;
double m_targetAngle;
// public static Gains m_aimGains;
public AimToCenter(Turret turret, SwerveDrive drive) {
@@ -36,32 +36,34 @@ public class AimToCenter extends CommandBase {
@Override
public void execute() {
if (x > 0) {
m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle();
}
m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
if (x < 0) {
m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle();
}
m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
if (x == 0 && y > 0) {
m_targetAngle = 270 - m_drive.gyro.getAngle();
m_targetAngle = 270 - m_drive.gyro.getAngle();
}
if (x == 0 && y < 0) {
m_targetAngle = 90 - m_drive.gyro.getAngle();
}
m_turret.runshooterRotatePID(m_targetAngle);
}
public boolean noIsDeadzone(){
public boolean isDeadzone() {
if ((-20 < m_targetAngle) && (m_targetAngle < 0)) {
return false;
return true;
} else {
return true;
return false;
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override