From fbbc41cf17babc550130591381061e3c00956d1c Mon Sep 17 00:00:00 2001
From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com>
Date: Sat, 20 Apr 2024 20:09:12 -0700
Subject: [PATCH 1/7] i tried adding babymode

---
 src/main/java/org/carlmontrobotics/Constants.java     | 11 +++++++++++
 src/main/java/org/carlmontrobotics/Robot.java         |  3 +++
 .../java/org/carlmontrobotics/RobotContainer.java     |  7 +++++++
 .../org/carlmontrobotics/commands/TeleopDrive.java    |  5 +++++
 .../java/org/carlmontrobotics/subsystems/Arm.java     |  7 +++++--
 5 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index 752f6eb2..3b071b13 100644
--- a/src/main/java/org/carlmontrobotics/Constants.java
+++ b/src/main/java/org/carlmontrobotics/Constants.java
@@ -145,8 +145,15 @@ public static final class Armc {
 		// fine for now, change it later before use - ("Incorect use of setIZone()"
 		// Issue #22)
 		// public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s
+		public static final double MAX_FF_VEL_RAD_P_S = Math.PI * .5;
 		public static final double MAX_FF_ACCEL_RAD_P_S = 53.728 / 4; // rad / s^2
 																		// ((.89*2)/(1.477/(61.875^2))/61.875)-20.84
+
+		public static final double MAX_FF_VEL_RAD_P_S_BABY = 0;
+		public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0;
+		//TODO: determine these values^
+
+
 		public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0;
 		public static final double CLIMB_POS = 1.701; //RADIANS
 		public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12);
@@ -309,6 +316,10 @@ public static final class Drivetrainc {
 		public static double kNormalDriveRotation = 0.5; // Percent Multiplier
 		public static double kSlowDriveSpeed = 0.4; // Percent Multiplier
 		public static double kSlowDriveRotation = 0.250; // Percent Multiplier
+
+		//baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use
+		public static double kBabyDriveSpeed = 0.3;
+		public static double kBabyDriveRotation = 0.2;
 		public static double kAlignMultiplier = 1D / 3D;
 		public static final double kAlignForward = 0.6;
 
diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java
index 09b83ebf..5f87aa9e 100644
--- a/src/main/java/org/carlmontrobotics/Robot.java
+++ b/src/main/java/org/carlmontrobotics/Robot.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -32,11 +33,13 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
     FollowPathCommand.warmupCommand().schedule();
+    
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    
   }
 
   @Override
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
index 9eeba3a8..6ade348a 100644
--- a/src/main/java/org/carlmontrobotics/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -63,6 +63,8 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
+  public static boolean babyMode = false;
+  
   // 1. using GenericHID allows us to use different kinds of controllers
   // 2. Use absolute paths from constants to reduce confusion
   public final GenericHID driverController = new GenericHID(Driver.port);
@@ -117,6 +119,8 @@ public class RobotContainer {
 
   public RobotContainer() {
     {
+      SmartDashboard.putBoolean("babymode", babyMode);
+      SmartDashboard.setPersistent("babymode");
       //safe auto setup... stuff in setupAutos() is not safe to run here - will break robot
       registerAutoCommands();
       SmartDashboard.putData(autoSelector);
@@ -142,6 +146,9 @@ public RobotContainer() {
     //setBindingsManipulatorENDEFF();
     setBindingsManipulatorNEO();
   }
+  
+
+ 
 
   private void setDefaultCommands() {
     drivetrain.setDefaultCommand(new TeleopDrive(
diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
index c773171e..f0310af7 100644
--- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
+++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
@@ -8,6 +8,7 @@
 import org.carlmontrobotics.Constants;
 import org.carlmontrobotics.Robot;
 import org.carlmontrobotics.subsystems.Drivetrain;
+import static org.carlmontrobotics.RobotContainer.*;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -94,6 +95,10 @@ public double[] getRequestedSpeeds() {
 
     double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed;
     double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation;
+    if(babyMode == true){
+      driveMultiplier = kBabyDriveSpeed;
+      rotationMultiplier = kBabyDriveRotation;
+    }
     // double driveMultiplier = kNormalDriveSpeed;
     // double rotationMultiplier = kNormalDriveRotation;
 
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 451079cb..18fd5e1d 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -11,6 +11,9 @@
 import org.carlmontrobotics.commands.TeleopArm;
 import org.carlmontrobotics.lib199.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
+import static org.carlmontrobotics.RobotContainer.*;
+
+import org.carlmontrobotics.RobotContainer;
 
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkBase.IdleMode;
@@ -113,8 +116,8 @@ public Arm() {
         armPIDMaster.setIZone(IZONE_RAD);
 
         TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-                Math.PI * .5,
-                MAX_FF_ACCEL_RAD_P_S);
+                (babyMode)? MAX_FF_VEL_RAD_P_S_BABY: MAX_FF_VEL_RAD_P_S,
+                (babyMode)?MAX_FF_ACCEL_RAD_P_S_BABY: MAX_FF_ACCEL_RAD_P_S);
         // ^ worst case scenario
         // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
         armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);

From b92542c24e6f77f371d6e73b9991e1105614f734 Mon Sep 17 00:00:00 2001
From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com>
Date: Sun, 21 Apr 2024 08:34:34 -0700
Subject: [PATCH 2/7] add voltage limits

---
 src/main/java/org/carlmontrobotics/Constants.java  |  2 ++
 .../java/org/carlmontrobotics/subsystems/Arm.java  | 14 ++++++++++++++
 2 files changed, 16 insertions(+)

diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index 3b071b13..2a841413 100644
--- a/src/main/java/org/carlmontrobotics/Constants.java
+++ b/src/main/java/org/carlmontrobotics/Constants.java
@@ -158,6 +158,8 @@ public static final class Armc {
 		public static final double CLIMB_POS = 1.701; //RADIANS
 		public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12);
 		public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12;
+		public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7;
+		public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7;
 		public static final double CLIMB_FINISH_POS = -0.38;
 		// if needed
 		public static final double COM_ARM_LENGTH_METERS = 0.381;
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 18fd5e1d..355d1a17 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -46,6 +46,7 @@
 
 // Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2
 public class Arm extends SubsystemBase {
+    private double pidMultiplier = 1;
     private boolean callDrive = true;
     private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER,
             MotorConfig.NEO);
@@ -118,6 +119,8 @@ public Arm() {
         TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
                 (babyMode)? MAX_FF_VEL_RAD_P_S_BABY: MAX_FF_VEL_RAD_P_S,
                 (babyMode)?MAX_FF_ACCEL_RAD_P_S_BABY: MAX_FF_ACCEL_RAD_P_S);
+
+        
         // ^ worst case scenario
         // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
         armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
@@ -149,6 +152,7 @@ public Arm() {
 
         SmartDashboard.putBoolean("arm is at pos", false);
     }
+    
 
     public void setBooleanDrive(boolean climb) {
         callDrive = climb;
@@ -231,9 +235,19 @@ public void periodic() {
             armMotorMaster.set(0);
             armMotorFollower.set(0);
         }
+        
+       
+        
 
         autoCancelArmCommand();
 
+        if(SmartDashboard.getBoolean("babymode", babyMode) == true){
+            armPIDMaster.setOutputRange(MIN_VOLTAGE_BABY, MAX_VOLTAGE_BABY);
+        }
+        else{
+            armPIDMaster.setOutputRange(MIN_VOLTAGE, MAX_VOLTAGE);
+        }
+
     }
     public static void setSelector(int num) {
         numSelector = num;

From cae9acf48df1f21d7cbe710b83294259e9017c3e Mon Sep 17 00:00:00 2001
From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com>
Date: Sun, 21 Apr 2024 13:07:20 -0700
Subject: [PATCH 3/7] moved constraings

---
 .../org/carlmontrobotics/subsystems/Arm.java    | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 355d1a17..687ec35c 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -46,7 +46,7 @@
 
 // Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2
 public class Arm extends SubsystemBase {
-    private double pidMultiplier = 1;
+    
     private boolean callDrive = true;
     private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER,
             MotorConfig.NEO);
@@ -116,14 +116,7 @@ public Arm() {
         armPIDMaster.setPositionPIDWrappingMaxInput((3 * Math.PI) / 2);
         armPIDMaster.setIZone(IZONE_RAD);
 
-        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-                (babyMode)? MAX_FF_VEL_RAD_P_S_BABY: MAX_FF_VEL_RAD_P_S,
-                (babyMode)?MAX_FF_ACCEL_RAD_P_S_BABY: MAX_FF_ACCEL_RAD_P_S);
-
         
-        // ^ worst case scenario
-        // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
-        armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
 
         SmartDashboard.putData("Arm", this);
 
@@ -160,6 +153,14 @@ public void setBooleanDrive(boolean climb) {
 
     @Override
     public void periodic() {
+        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
+                (babyMode)? MAX_FF_VEL_RAD_P_S_BABY: MAX_FF_VEL_RAD_P_S,
+                (babyMode)?MAX_FF_ACCEL_RAD_P_S_BABY: MAX_FF_ACCEL_RAD_P_S);
+
+        
+        // ^ worst case scenario
+        // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
+        armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
         SmartDashboard.putData(this);
         // armMotorMaster.setSmartCurrentLimit(50);
         // armMotorFollower.setSmartCurrentLimit(50);

From b365f63d87d29270590c2d14f9490b63790f5ce2 Mon Sep 17 00:00:00 2001
From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com>
Date: Sun, 21 Apr 2024 16:11:45 -0700
Subject: [PATCH 4/7] changed to setDefaultBoolean

---
 src/main/java/org/carlmontrobotics/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
index 6ade348a..fe0c3cdc 100644
--- a/src/main/java/org/carlmontrobotics/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -119,7 +119,7 @@ public class RobotContainer {
 
   public RobotContainer() {
     {
-      SmartDashboard.putBoolean("babymode", babyMode);
+      SmartDashboard.setDefaultBoolean("babymode", babyMode);
       SmartDashboard.setPersistent("babymode");
       //safe auto setup... stuff in setupAutos() is not safe to run here - will break robot
       registerAutoCommands();

From 8a57206429d788f0bf85c32557fbe5d033b5f250 Mon Sep 17 00:00:00 2001
From: DriverStationComputer <deepbluerobotics@gmail.com>
Date: Sun, 21 Apr 2024 19:15:33 -0700
Subject: [PATCH 5/7] fixed and tested babymode

---
 .../java/org/carlmontrobotics/RobotContainer.java  |  2 +-
 .../java/org/carlmontrobotics/commands/Climb.java  |  7 +++++--
 .../org/carlmontrobotics/commands/RampMaxRPM.java  |  5 ++++-
 .../org/carlmontrobotics/commands/TeleopDrive.java |  3 ++-
 .../java/org/carlmontrobotics/subsystems/Arm.java  | 14 ++++++++------
 5 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
index fe0c3cdc..b19f0aaf 100644
--- a/src/main/java/org/carlmontrobotics/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -63,7 +63,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
-  public static boolean babyMode = false;
+  private static boolean babyMode = false;
   
   // 1. using GenericHID allows us to use different kinds of controllers
   // 2. Use absolute paths from constants to reduce confusion
diff --git a/src/main/java/org/carlmontrobotics/commands/Climb.java b/src/main/java/org/carlmontrobotics/commands/Climb.java
index fda313de..bb1682a1 100644
--- a/src/main/java/org/carlmontrobotics/commands/Climb.java
+++ b/src/main/java/org/carlmontrobotics/commands/Climb.java
@@ -14,6 +14,7 @@ public class Climb extends Command {
     // correctly
     private Arm arm;
     private Timer timer = new Timer();
+    private boolean babyMode;
     public Climb(Arm arm) {
         this.arm = arm;
         addRequirements(arm);
@@ -22,15 +23,17 @@ public Climb(Arm arm) {
 
     @Override
     public void initialize() {
+        babyMode = SmartDashboard.getBoolean("babymode", false);
         arm.setLimitsForClimbOn();
         arm.setBooleanDrive(false);
     }
 
     @Override
     public void execute() {
+        if(!babyMode) {
         arm.driveArm(-12);
         timer.start();
-        
+        }
     }
     
     @Override
@@ -48,6 +51,6 @@ public void end(boolean interrupted) {
     @Override
     public boolean isFinished() {
         // TODO: Figure out the actual climb position
-        return arm.getArmPos() - (CLIMB_FINISH_POS) < Units.degreesToRadians(0.1) || timer.get() >= 5;
+        return arm.getArmPos() - (CLIMB_FINISH_POS) < Units.degreesToRadians(0.1) || timer.get() >= 10 || babyMode;
     }
 }
\ No newline at end of file
diff --git a/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java b/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java
index 8ae9f319..0312fc6c 100644
--- a/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java
+++ b/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java
@@ -6,12 +6,14 @@
 import org.carlmontrobotics.subsystems.IntakeShooter;
 
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 // TODO: where do we use this command?
 public class RampMaxRPM extends Command {
   // intake until sees game peice or 4sec has passed
   private final IntakeShooter intake;
+  private boolean babyMode;
   private Timer timer;
 
   public RampMaxRPM(IntakeShooter intake) {
@@ -20,6 +22,7 @@ public RampMaxRPM(IntakeShooter intake) {
 
   @Override
   public void initialize() {
+    babyMode = SmartDashboard.getBoolean("babymode", false);
     intake.setMaxOutake(1);
     
   }
@@ -39,6 +42,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return false || babyMode;
   }
 }
diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
index f0310af7..db127767 100644
--- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
+++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java
@@ -27,7 +27,7 @@ public class TeleopDrive extends Command {
   private double currentForwardVel = 0;
   private double currentStrafeVel = 0;
   private double prevTimestamp;
-
+  private boolean babyMode;
   /**
    * Creates a new TeleopDrive.
    */
@@ -58,6 +58,7 @@ public void execute() {
     double[] speeds = getRequestedSpeeds();
     // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp);
     prevTimestamp = currentTime;
+    babyMode = SmartDashboard.getBoolean("babymode", false);
     // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation);
     // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed);
     // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation);
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 687ec35c..1676db74 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -77,7 +77,7 @@ public class Arm extends SubsystemBase {
     private final MutableMeasure<Voltage> voltage = mutable(Volts.of(0));
     private final MutableMeasure<Velocity<Angle>> velocity = mutable(RadiansPerSecond.of(0));
     private final MutableMeasure<Angle> distance = mutable(Radians.of(0));
-
+    private static boolean babyMode;
     private ShuffleboardTab sysIdTab = Shuffleboard.getTab("arm SysID");
     private boolean setPIDOff; 
     public Arm() {
@@ -153,11 +153,12 @@ public void setBooleanDrive(boolean climb) {
 
     @Override
     public void periodic() {
+        babyMode = SmartDashboard.getBoolean("babymode", false);
         TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-                (babyMode)? MAX_FF_VEL_RAD_P_S_BABY: MAX_FF_VEL_RAD_P_S,
-                (babyMode)?MAX_FF_ACCEL_RAD_P_S_BABY: MAX_FF_ACCEL_RAD_P_S);
+                (MAX_FF_VEL_RAD_P_S),
+                (MAX_FF_ACCEL_RAD_P_S));
 
-        
+        //Aaron was here
         // ^ worst case scenario
         // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
         armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
@@ -243,12 +244,13 @@ public void periodic() {
         autoCancelArmCommand();
 
         if(SmartDashboard.getBoolean("babymode", babyMode) == true){
-            armPIDMaster.setOutputRange(MIN_VOLTAGE_BABY, MAX_VOLTAGE_BABY);
+            armPIDMaster.setOutputRange(-0.4/12, 0.4/12);
         }
         else{
-            armPIDMaster.setOutputRange(MIN_VOLTAGE, MAX_VOLTAGE);
+            armPIDMaster.setOutputRange(MIN_VOLTAGE/12, MAX_VOLTAGE/12);
         }
 
+
     }
     public static void setSelector(int num) {
         numSelector = num;

From 71901c27e8cfe2a0edb1bd8ac374040c5bc96a17 Mon Sep 17 00:00:00 2001
From: Team 199 Driver Station Computer
 <35879629+DriverStationComputer@users.noreply.github.com>
Date: Thu, 25 Apr 2024 17:24:22 -0700
Subject: [PATCH 6/7] Fixed periodic issues with output range and trap
 constraints

---
 .../org/carlmontrobotics/subsystems/Arm.java  | 21 +++++++++----------
 1 file changed, 10 insertions(+), 11 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 1676db74..5573c4a4 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -142,7 +142,15 @@ public Arm() {
         // SmartDashboard.putNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS);
         armMotorMaster.setSmartCurrentLimit(80);
         armMotorFollower.setSmartCurrentLimit(80);
-
+        if(SmartDashboard.getBoolean("babymode", babyMode) == true){
+            armPIDMaster.setOutputRange(-0.3/12, 0.3/12);
+        }
+        else{
+            armPIDMaster.setOutputRange(MIN_VOLTAGE/12, MAX_VOLTAGE/12);
+        }
+        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
+                (MAX_FF_VEL_RAD_P_S),
+                (MAX_FF_ACCEL_RAD_P_S));
         SmartDashboard.putBoolean("arm is at pos", false);
     }
     
@@ -154,9 +162,7 @@ public void setBooleanDrive(boolean climb) {
     @Override
     public void periodic() {
         babyMode = SmartDashboard.getBoolean("babymode", false);
-        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-                (MAX_FF_VEL_RAD_P_S),
-                (MAX_FF_ACCEL_RAD_P_S));
+        
 
         //Aaron was here
         // ^ worst case scenario
@@ -243,13 +249,6 @@ public void periodic() {
 
         autoCancelArmCommand();
 
-        if(SmartDashboard.getBoolean("babymode", babyMode) == true){
-            armPIDMaster.setOutputRange(-0.4/12, 0.4/12);
-        }
-        else{
-            armPIDMaster.setOutputRange(MIN_VOLTAGE/12, MAX_VOLTAGE/12);
-        }
-
 
     }
     public static void setSelector(int num) {

From 050db6ff6dc35f8ceeabf7336c8624d8af89463a Mon Sep 17 00:00:00 2001
From: Team 199 Driver Station Computer
 <35879629+DriverStationComputer@users.noreply.github.com>
Date: Thu, 25 Apr 2024 17:26:10 -0700
Subject: [PATCH 7/7] oops

---
 src/main/java/org/carlmontrobotics/subsystems/Arm.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 793fa4c2..b93d30bf 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -157,10 +157,12 @@ public Arm() {
         TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
                 (MAX_FF_VEL_RAD_P_S),
                 (MAX_FF_ACCEL_RAD_P_S));
+        armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
         SmartDashboard.putBoolean("arm is at pos", false);
         if (RobotBase.isSimulation()) {
             rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations");
         }
+
     }
     
 
@@ -176,7 +178,7 @@ public void periodic() {
         //Aaron was here
         // ^ worst case scenario
         // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
-        armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
+
         SmartDashboard.putData(this);
         // armMotorMaster.setSmartCurrentLimit(50);
         // armMotorFollower.setSmartCurrentLimit(50);