FIXED ARM COMMANDS AND ADDED VORTEX OUTTAKE

DeepBlueRobotics · Mar 27, 2024 · f68c54b · f68c54b
1 parent 20e48ae
commit f68c54b
Show file tree

Hide file tree

Showing 6 changed files with 32 additions and 25 deletions.
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -193,10 +193,12 @@ private void setBindingsManipulatorENDEFF() {
         //new MoveToPos(arm, GROUND_INTAKE_POS)
        //MoveToPos(arm, GROUND_INTAKE_POS));
 
-      new JoystickButton(manipulatorController, Button.kX.value).onTrue(new MoveToPos(arm, Armc.UPPER_ANGLE_LIMIT_RAD));
+      //new JoystickButton(manipulatorController, Button.kX.value).onTrue(new MoveToPos(arm, Armc.UPPER_ANGLE_LIMIT_RAD));
       new JoystickButton(manipulatorController, Button.kB.value).onTrue(new ClimbArmSoftLimit(arm));
     new JoystickButton(manipulatorController, Button.kLeftStick.value).onTrue(new GETOUT(intakeShooter));
-    //NEW BINDINGS(easier for manipulator)
+    new JoystickButton(manipulatorController, Button.kRightStick.value).onTrue(new MoveToPos(arm, -0.2));
+    //NEW BINDINGS(easier for mani
+
     //Xbox left joy Y axis -> raw Intake/Outtake control
     //Xbox right joy Y axis -> raw Arm control
     //Xbox right trigger axis -> Intake pos + intake

diff --git a/src/main/java/org/carlmontrobotics/commands/ClimbArmSoftLimit.java b/src/main/java/org/carlmontrobotics/commands/ClimbArmSoftLimit.java
@@ -4,11 +4,8 @@
 
 import static org.carlmontrobotics.Constants.Armc.*;
 
-import org.carlmontrobotics.Constants.Armc.*;
 
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
@@ -27,18 +24,19 @@ public void initialize() {
     }
     @Override
     public void execute() {
-        arm.driveArm(SmartDashboard.getNumber("climber volts", 0));
+        arm.driveArm(-12);
+
     }
     @Override
     public void end(boolean interrupted) {
         arm.stopArm(); //OR maintain small voltage to keep arm in place?
         arm.resetSoftLimit();
-        arm.setArmTarget(arm.getArmPos());
+        // arm.setArmTarget(arm.getArmPos());
         arm.setBooleanDrive(true);
     }
     @Override
     public boolean isFinished() {
         //TODO: Figure out the actual climb position
-        return Math.abs(arm.getArmPos() - CLIMBER_DOWN_ANGLE_RAD) < Units.degreesToRadians(5);
+        return Math.abs(arm.getArmPos() - GROUND_INTAKE_POS) < Units.degreesToRadians(5);
     }
 }
diff --git a/src/main/java/org/carlmontrobotics/commands/GETOUT.java b/src/main/java/org/carlmontrobotics/commands/GETOUT.java
@@ -24,6 +24,7 @@ public void execute() {
     public void end(boolean interrupted) {
         timer.stop();
         timer.reset();
+        intakeShooter.setCurrentLimit(20);
     }
     @Override
     public boolean isFinished() {

diff --git a/src/main/java/org/carlmontrobotics/commands/PassToOutake.java b/src/main/java/org/carlmontrobotics/commands/PassToOutake.java
@@ -37,7 +37,7 @@ public void initialize(){
   @Override
   public void execute() {
     //intake.setMaxOutake();
-    if (intake.getOutakeRPM()>=TEST_RPM - 1000) {//SPEAKER_RPM){
+    if (intake.getOutakeRPM()>=4200) {//SPEAKER_RPM){
       intake.setMaxIntake(1);
       timer.start();
     }

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -152,6 +152,9 @@ public Arm() {
 
         SmartDashboard.putNumber("ramp rate (s)", 2);
         SmartDashboard.putNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS);
+        armMotorMaster.setSmartCurrentLimit(80);
+        armMotorFollower.setSmartCurrentLimit(80);
+
     }
     public void setBooleanDrive(boolean climb) {
         callDrive = climb;
@@ -160,8 +163,8 @@ public void setBooleanDrive(boolean climb) {
     @Override
     public void periodic() {
 
-        armMotorMaster.setSmartCurrentLimit(50);
-        armMotorFollower.setSmartCurrentLimit(50);
+        // armMotorMaster.setSmartCurrentLimit(50);
+        // armMotorFollower.setSmartCurrentLimit(50);
         if (DriverStation.isDisabled())
             resetGoal();
 

diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java
@@ -41,11 +41,11 @@
 
 public class IntakeShooter extends SubsystemBase {
     private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO_550);
-    private final CANSparkMax outakeMotor = MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550);
-    //private final CANSparkFlex outakeMotorVortex = new CANSparkFlex(10,MotorType.kBrushless);
-    private final RelativeEncoder outakeEncoder = outakeMotor.getEncoder();
+   // private final CANSparkMax outakeMotor = MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550);
+    private final CANSparkFlex outakeMotorVortex = new CANSparkFlex(10,MotorType.kBrushless);
+    private final RelativeEncoder outakeEncoder = outakeMotorVortex.getEncoder();
     private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder();
-    private final SparkPIDController pidControllerOutake = outakeMotor.getPIDController();
+    private final SparkPIDController pidControllerOutake = outakeMotorVortex.getPIDController();
     private final SparkPIDController pidControllerIntake = intakeMotor.getPIDController();
     private Timer timer = new Timer();
     private int count = 0;
@@ -63,7 +63,7 @@ public class IntakeShooter extends SubsystemBase {
     public IntakeShooter() {
         //Figure out which ones to set inverted
         intakeMotor.setInverted(INTAKE_MOTOR_INVERSION);
-        outakeMotor.setInverted(OUTAKE_MOTOR_INVERSION);
+        outakeMotorVortex.setInverted(OUTAKE_MOTOR_INVERSION);
         pidControllerOutake.setP(kP[OUTTAKE]);
         pidControllerOutake.setI(kI[OUTTAKE]);
         pidControllerOutake.setD(kD[OUTTAKE]);
@@ -74,6 +74,7 @@ public IntakeShooter() {
         intakeEncoder.setAverageDepth(4);
         intakeEncoder.setMeasurementPeriod(8);
         SmartDashboard.putNumber("intake volts", 0);
+        SmartDashboard.putNumber("Vortex volts", 0);
        // setMaxOutakeOverload(1);
 
     }
@@ -90,7 +91,7 @@ private double getGamePieceDistanceIntake() {
         return Units.metersToInches(intakeDistanceSensor.getRange()/1000) - DS_DEPTH_INCHES;
     }
     public void motorSetOutake(int speed) {
-        outakeMotor.set(speed);
+        outakeMotorVortex.set(speed);
     }
     private double getGamePieceDistanceOutake() {
         return Units.metersToInches(OutakeDistanceSensor.getRange()/1000) - DS_DEPTH_INCHES;
@@ -133,11 +134,12 @@ public boolean outakeDetectsNote() {
 
     @Override
     public void periodic() {
-        outakeMotor.set(SmartDashboard.getNumber("intake volts", 0));
-        SmartDashboard.putNumber("outtake vel", outakeMotor.getEncoder().getVelocity());
+        //outakeMotor.set(SmartDashboard.getNumber("intake volts", 0));
+        SmartDashboard.putNumber("outtake vel", outakeMotorVortex.getEncoder().getVelocity());
+
         //count++;
-        //double volts = SmartDashboard.getNumber("Vortex volts", 0);
-        //outakeMotorVortex.set(volts);
+        double volts = SmartDashboard.getNumber("Vortex volts", 0);
+       // outakeMotorVortex.set(volts);
 
       //setMaxOutake();
 
@@ -154,16 +156,16 @@ public void setMaxIntake(int direction) {
 
     }
     public void setMaxOutakeOverload(int direction) {
-        outakeMotor.setSmartCurrentLimit(40);
+     //   outakeMotor.setSmartCurrentLimit(40);
         //outakeMotor.setSmartCurrentLimit(1*direction);
     }
     public void setMaxOutake() {
-     outakeMotor.set(1);   
+     outakeMotorVortex.set(1);   
     }
 
     public void resetCurrentLimit() {
         intakeMotor.setSmartCurrentLimit(MotorConfig.NEO_550.currentLimitAmps);
-        outakeMotor.setSmartCurrentLimit(MotorConfig.NEO.currentLimitAmps);
+       // outakeMotorVortex.setSmartCurrentLimit(MotorConfig.NEO.currentLimitAmps);
         //intakeMotor.setSmartCurrentLimit(MotorConfig.NEO_550.currentLimitAmps);
     }
 
@@ -184,7 +186,8 @@ public void stopOutake() {
     }
 
     public void stopIntake() {
-        outakeMotor.set(0);
+        intakeMotor.set(0);
+        outakeMotorVortex.set(0);
     }
     public double getIntakeRPM() {
         return intakeEncoder.getVelocity();