From 1916a9d39a72ac4d03561c832a5a0f58b68802ba Mon Sep 17 00:00:00 2001
From: siyoyoCode <siyona101215@gmail.com>
Date: Sat, 27 Jan 2024 14:47:47 -0800
Subject: [PATCH] testing shooter code

---
 src/main/java/frc/robot/RobotContainer.java   |  3 ++-
 .../subsystems/ShooterPivotSubsystem.java     | 19 ++++++++++++++-----
 2 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 94655ec2..c442b9a9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -119,7 +119,8 @@ private void configureBindings() {
         }));
 
         pivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
-            pivotSubsystem.setPivotMotorSpeed(.3 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+            pivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()));
+            // pivotSubsystem.printCurrentAngle();
         }, pivotSubsystem));
 
         if(baseSwerveSubsystem instanceof SwerveSubsystem){
diff --git a/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java
index 2f406fb9..8daee51c 100644
--- a/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterPivotSubsystem.java
@@ -17,6 +17,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     final double GEARBOX_RATIO = 18.16; //ask cadders
     public final int ERRORTOLERANCE = 5; //error tolerance for pid
     final int LIMIT_SWITCH_ID = 1; //placeholder
+    final double CONVERSION_FACTOR = Math.PI/(2*4.57);
 
     //motors
     private final CANSparkMax pivotMotor;
@@ -49,6 +50,7 @@ public ShooterPivotSubsystem(boolean alliance){
 
         //motors
         pivotMotor = new CANSparkMax(12, MotorType.kBrushless); 
+        pivotMotor.setInverted(true);
 
         //devices
         rotationEncoder = pivotMotor.getEncoder();
@@ -63,7 +65,8 @@ public ShooterPivotSubsystem(boolean alliance){
         rotationPIDController.setD(ANGLE_D);
 
         //encoder stuff
-        rotationEncoder.setPositionConversionFactor(GEARBOX_RATIO);
+        rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
+        //rotationEncoder.setInverted(true);
         rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); //what does 0 do (slotID is from 0-3)
 
         //field
@@ -94,6 +97,10 @@ public double getAutoAimAngle(double distance){
         return Math.atan(speakerHeight/distance);
     }
 
+    public void printCurrentAngle(){
+        System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29);
+    }
+
     public double getDistance(){
 
         if(alliance){ //true = red
@@ -126,15 +133,17 @@ public void setAutoAimBoolean(boolean red){
     public void periodic(){
         //resets relative encoder every time robot starts again
         //(check if encoder prints zero when run)
-        if(limitSwitch != null && limitSwitch.get()){ //false = limit switch is pressed
-            rotationEncoder.setPosition(0); 
-            // System.out.println(rotationEncoder.getPosition()); //should print 0
-        }
+        // if(limitSwitch != null && limitSwitch.get()){ //false = limit switch is pressed
+        //     rotationEncoder.setPosition(0); 
+        //     // System.out.println(rotationEncoder.getPosition()); //should print 0
+        // }
 
         if(autoAim){
             setAngle(getAutoAimAngle(getDistance()));
         }
 
+        printCurrentAngle();
+
         // System.out.println("current pos" + rotationEncoder.getPosition());
 
         // if(currentState == ShooterState.FIRING && (shooterSensor.getRed() < TOLERANCE)){  //when there is no note