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

Cleaned up Math, added unit testing to AimToCenter

Browse Source
This commit is contained in:
Ryan Manley
2022-02-26 13:48:50 -07:00
parent 31346ff646
commit 350cf33dd0
9 changed files with 262 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+13 -20
View File
@@ -17,6 +17,7 @@ public class AimToCenter extends CommandBase {
// use odometry to find x and y later
double x = 0;
double y = 0;
double angle = 0;
double m_targetAngle;
// public static Gains m_aimGains;
@@ -31,34 +32,26 @@ public class AimToCenter extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = 0;
y = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (x > 0) {
m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
else if (x < 0) {
m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
else if (x == 0 && y > 0) {
m_targetAngle = 270 - m_drive.gyro.getAngle();
}
else if (x == 0 && y < 0) {
m_targetAngle = 90 - m_drive.gyro.getAngle();
}
m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle());
m_turret.runshooterRotatePID(m_targetAngle);
}
public boolean isDeadzone() {
if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) {
return true;
} else {
return false;
}
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return(angle);
//m_turret.runshooterRotatePID(m_targetAngle);
}
public static boolean isDeadzone(double angle) {
if ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)) return true;
else return false;
}
// Called once the command ends or is interrupted.