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

updated aim to center

Browse Source
This commit is contained in:
Abhishrek05
2022-02-19 15:05:49 -07:00
parent 64481cb1cc
commit 92777c105d
2 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+13 -1
View File
@@ -36,7 +36,19 @@ public class AimToCenter extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x);
if (x > 0) {
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();
}
if (x == 0 && y > 0) {
m_targetAngle = 270 - m_drive.gyro.getAngle();
}
if (x == 0 && y < 0) {
m_targetAngle = 90 - m_drive.gyro.getAngle();
}
m_turret.runshooterRotatePID(m_targetAngle);
}