From 494eea0838a49f22e972c4216b6c0069b8f4067b Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 1 Mar 2024 22:57:06 -0800 Subject: [PATCH 1/2] removed commands --- .../carlmontrobotics/commands/armAmpPos.java | 47 ------------------ .../commands/armClimberDownPos.java | 47 ------------------ .../commands/armClimberUpPos.java | 47 ------------------ .../commands/armGroundPos.java | 47 ------------------ .../commands/armNextToSpeakerPos.java | 48 ------------------- .../commands/armPodiumSpeakerPos.java | 47 ------------------ .../commands/armSafeZonePos.java | 48 ------------------- .../org/carlmontrobotics/subsystems/Arm.java | 1 - 8 files changed, 332 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/armAmpPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armClimberDownPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armClimberUpPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armGroundPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armNextToSpeakerPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armPodiumSpeakerPos.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/armSafeZonePos.java diff --git a/src/main/java/org/carlmontrobotics/commands/armAmpPos.java b/src/main/java/org/carlmontrobotics/commands/armAmpPos.java deleted file mode 100644 index f245580e..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armAmpPos.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armAmpPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armAmpPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 1); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/armClimberDownPos.java b/src/main/java/org/carlmontrobotics/commands/armClimberDownPos.java deleted file mode 100644 index c0fb024a..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armClimberDownPos.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armClimberDownPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armClimberDownPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 6); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/armClimberUpPos.java b/src/main/java/org/carlmontrobotics/commands/armClimberUpPos.java deleted file mode 100644 index 17f529c1..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armClimberUpPos.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armClimberUpPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armClimberUpPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 5); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/armGroundPos.java b/src/main/java/org/carlmontrobotics/commands/armGroundPos.java deleted file mode 100644 index d69c0392..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armGroundPos.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armGroundPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armGroundPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 0); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/armNextToSpeakerPos.java b/src/main/java/org/carlmontrobotics/commands/armNextToSpeakerPos.java deleted file mode 100644 index e6c4cefc..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armNextToSpeakerPos.java +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armNextToSpeakerPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armNextToSpeakerPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 3); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} - diff --git a/src/main/java/org/carlmontrobotics/commands/armPodiumSpeakerPos.java b/src/main/java/org/carlmontrobotics/commands/armPodiumSpeakerPos.java deleted file mode 100644 index a8801d73..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armPodiumSpeakerPos.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armPodiumSpeakerPos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armPodiumSpeakerPos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 2); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/armSafeZonePos.java b/src/main/java/org/carlmontrobotics/commands/armSafeZonePos.java deleted file mode 100644 index a34eaa21..00000000 --- a/src/main/java/org/carlmontrobotics/commands/armSafeZonePos.java +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.Arm; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class armSafeZonePos extends Command { - /** Creates a new armPosAmp. */ - private Arm arm; - private Timer armProfileTimer = new Timer(); - public armSafeZonePos(Arm arm) { - // Use addRequirements() here to declare subsystem dependencies. - this.arm = arm; - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - armProfileTimer.reset(); - armProfileTimer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 4); - arm.COMBINE_PID_FF_TRAPEZOID(setpoint); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - armProfileTimer.stop(); - armProfileTimer.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} - diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 39f6254d..46838de5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -4,7 +4,6 @@ import org.carlmontrobotics.commands.ArmTeleop; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; - import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMax; From e472500fa92d473ae9cddb6191d9cc7fe3f61330 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 1 Mar 2024 23:01:57 -0800 Subject: [PATCH 2/2] fixed requested speeds --- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 46838de5..2d8333b0 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -123,19 +123,19 @@ public void periodic() { autoCancelArmCommand(); } - /* public void autoCancelArmCommand() { + public void autoCancelArmCommand() { if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return; - double[] requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds(); + double requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds(); - if(requestedSpeeds[0] != 0 || requestedSpeeds[1] != 0) { + if(requestedSpeeds != 0) { Command currentArmCommand = getCurrentCommand(); if(currentArmCommand != getDefaultCommand() && currentArmCommand != null) { currentArmCommand.cancel(); } } } -*/ + //#region Drive Methods public void driveArm(double goalAngle) {