added target in turretpoint.java back deleted faketargetangle

StuyPulse · Dec 13, 2023 · c642e8e · c642e8e
1 parent fde7999
commit c642e8e
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 18 deletions.
diff --git a/src/main/java/com/stuypulse/robot/commands/TurretPoint.java b/src/main/java/com/stuypulse/robot/commands/TurretPoint.java
@@ -15,7 +15,7 @@ public class TurretPoint extends CommandBase {
     private Translation2d target;
 
     public TurretPoint(Translation2d target) {
-        //this.target = target;
+        this.target = target;
 
         turret = Turret.getInstance();
         odometry = Odometry.getInstance();
@@ -27,25 +27,13 @@ public TurretPoint(Translation2d target) {
     public final void execute() {
         Pose2d robotPose = odometry.getPose();
 
-        // if ((Math.abs(robotPose.getX()) < .0000001) && (Math.abs(target.getX()) < .0000001)) {
-        //     turret.setTargetAngle(0, 360, -360);
-        // }
-        if (Math.abs(robotPose.getX()) < .0000001) {
-            if (robotPose.getY() < .0000001) {
-                turret.setTargetAngle(0, 360, -360);
-            }
-            else {
-                turret.setTargetAngle(
-            Math.atan(robotPose.getX() / robotPose.getY())    
-            , 360, -360);
-            }
+        if ((Math.abs(robotPose.getX()) < .0000001) && (Math.abs(target.getX()) < .0000001)) {
+            turret.setTargetAngle(0, 360, -360);
         }
-
         else {
             turret.setTargetAngle(
             (Math.toDegrees(
-                // Math.atan((robotPose.getY() - target.getY()) / (robotPose.getX() - target.getX()))
-                Math.atan(robotPose.getY() / robotPose.getX())
+                Math.atan((robotPose.getY() - target.getY()) / (robotPose.getX() - target.getX()))
             )), 360, -360
             );
         }

diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java
@@ -2,6 +2,7 @@
 
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Ports.Gamepad;
 import com.stuypulse.robot.constants.Settings.*;
 import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.control.feedback.PIDController;
@@ -29,7 +30,6 @@ public static Turret getInstance() {
 
     private Controller controller;
     private SmartNumber targetAngle = new SmartNumber("Target Angle", 120);
-    private SmartNumber fakeTargetAngle = new SmartNumber("Fake Target Angle", 0);
 
     public void stop() {
         setTurretVoltage(0);
@@ -63,7 +63,6 @@ public final void periodic() {
             getTurretAngle()
         );
 
-        setTargetAngle(fakeTargetAngle.get(), 360, -360);
 
         double output = controller.getOutput();
         setTurretVoltage(output);