Merge branch 'master' into add-config-support

src/main/java/org/carlmontrobotics/Constants.java

@@ -145,12 +145,21 @@ 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);
 		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;
@@ -309,6 +318,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;
 

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;
 
@@ -30,11 +31,13 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
     FollowPathCommand.warmupCommand().schedule();
+
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+
   }
 
   @Override

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -63,6 +63,8 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
+  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
   public final GenericHID driverController = new GenericHID(Driver.port);
@@ -120,7 +122,8 @@ public RobotContainer() {
       // Put any configuration overrides to the dashboard and the terminal
       SmartDashboard.putData("CONFIG overrides", Config.CONFIG);
       System.out.println(Config.CONFIG);
-
+      SmartDashboard.setDefaultBoolean("babymode", babyMode);
+      SmartDashboard.setPersistent("babymode");
       //safe auto setup... stuff in setupAutos() is not safe to run here - will break robot
       registerAutoCommands();
       SmartDashboard.putData(autoSelector);
@@ -146,6 +149,9 @@ public RobotContainer() {
     //setBindingsManipulatorENDEFF();
     setBindingsManipulatorNEO();
   }
+
+
+
 
   private void setDefaultCommands() {
     drivetrain.setDefaultCommand(new TeleopDrive(

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;
     }
 }

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;
   }
 }

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;
@@ -26,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.
    */
@@ -57,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);
@@ -94,6 +96,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;
 

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;
@@ -46,6 +49,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 boolean callDrive = true;
     private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER,
             MotorConfig.NEO);
@@ -76,7 +80,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; 
 
@@ -118,12 +122,7 @@ public Arm() {
         armPIDMaster.setPositionPIDWrappingMaxInput((3 * Math.PI) / 2);
         armPIDMaster.setIZone(IZONE_RAD);
 
-        TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
-                Math.PI * .5,
-                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);
 
@@ -149,19 +148,37 @@ 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));
+        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");
         }
+
     }
+
 
     public void setBooleanDrive(boolean climb) {
         callDrive = climb;
     }
 
     @Override
     public void periodic() {
+        babyMode = SmartDashboard.getBoolean("babymode", false);
+
+
+        //Aaron was here
+        // ^ worst case scenario
+        // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S)
+
         SmartDashboard.putData(this);
         // armMotorMaster.setSmartCurrentLimit(50);
         // armMotorFollower.setSmartCurrentLimit(50);
@@ -237,9 +254,13 @@ public void periodic() {
             armMotorMaster.set(0);
             armMotorFollower.set(0);
         }
+
+
+
 
         autoCancelArmCommand();
 
+
     }
     public static void setSelector(int num) {
         numSelector = num;