From b9ea6128cd6a87eb1ef748b57175d1cb613da298 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Mon, 12 Feb 2024 18:06:04 -0800 Subject: [PATCH 1/8] created action --- .../amp_trap/AmpTrapGotoAction.java | 99 +++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java new file mode 100644 index 0000000..f4d002e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -0,0 +1,99 @@ +package frc.robot.subsystems.amp_trap; + +import org.xero1425.base.actions.Action; +import org.xero1425.base.subsystems.motorsubsystem.MCMotionMagicAction; + +public class AmpTrapGotoAction extends Action { + private MCMotionMagicAction elevatorThreshAction; + private MCMotionMagicAction elevatorGotoAction; + private MCMotionMagicAction wristAction; + private AmpTrapSubsystem subsystem; + private double elevatorThreshold; + private double wristNoGoZoneLower; + private double wristNoGoZoneUpper; + private double wristTarget; + private String state; + + public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, double wristTarget) throws Exception{ + super(subsystem.getRobot().getMessageLogger()); + this.subsystem = subsystem; + + // Get the elevator threshold and the no-go zone limits for the wrist from the params file + this.elevatorThreshold = subsystem.getSettingsValue("elevatorThreshold").getDouble(); + this.wristNoGoZoneLower = subsystem.getSettingsValue("wristNoGoZoneLower").getDouble(); + this.wristNoGoZoneUpper = subsystem.getSettingsValue("wristNoGoZoneUpper").getDouble(); + + this.wristTarget = wristTarget; + + this.elevatorThreshAction = new MCMotionMagicAction(subsystem.getElevator(), "ElevatorThresh", elevatorThreshold, 0.01, 0.02); + this.elevatorGotoAction = new MCMotionMagicAction(subsystem.getWrist(), "ElevatorThresh", elevatorTarget, 0.01, 0.02); + + // Create the MCMotionMagicAction for the wrist motor + this.wristAction = new MCMotionMagicAction(subsystem.getWrist(), "WristGoto", wristTarget, 3, 10); + + state = "Thresh"; + } + + @Override + public void start() throws Exception{ + super.start(); + subsystem.getElevator().setAction(elevatorGotoAction); + subsystem.getElevator().setAction(elevatorThreshAction); + subsystem.getWrist().setAction(wristAction); + + // Start the MCMotionMagicActions + if(wristTarget < wristNoGoZoneLower && wristTarget > wristNoGoZoneUpper){ + wristAction.start(); + elevatorGotoAction.start(); + state = "Goto"; + }else{ + elevatorThreshAction.start(); + state = "Thresh"; + } + } + + @Override + public void run() throws Exception{ + super.run(); + // Update the MCMotionMagicActions + if(state.equals("Goto")){ + elevatorGotoAction.run(); + } + if(state.equals("Goto") || state.equals("WristGoto")){ + wristAction.run(); + } + if(state.equals("Thresh")){ + elevatorThreshAction.run(); + } + // If the elevator has moved up past the elevator threshold, move the wrist to its target position + if (elevatorThreshAction.isDone() && state.equals("Thresh")){ + wristAction.start(); + state = "WristGoto"; + } + + if(wristAction.isDone()){ + if(elevatorGotoAction.isDone() && state.equals("Goto")){ + setDone(); + return; + } + if(state.equals("WristGoto")){ + elevatorGotoAction.start(); + state = "Goto"; + } + } + } + + @Override + public void cancel() { + super.cancel(); + // Cancel the MCMotionMagicActions + elevatorGotoAction.cancel(); + elevatorThreshAction.cancel(); + wristAction.cancel(); + } + + @Override + public String toString(int indent) { + return prefix(indent) + "AmpTrapGotoAction"; + } +} From 742f317a4e7310891367bc952997cf5ba0c951c5 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Mon, 12 Feb 2024 18:09:59 -0800 Subject: [PATCH 2/8] updated, realized that something was wrong cuz i wasnt using the subsystem --- .../robot/subsystems/amp_trap/AmpTrapGotoAction.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index f4d002e..57187af 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -43,11 +43,11 @@ public void start() throws Exception{ // Start the MCMotionMagicActions if(wristTarget < wristNoGoZoneLower && wristTarget > wristNoGoZoneUpper){ - wristAction.start(); - elevatorGotoAction.start(); + subsystem.getWrist().setAction(wristAction); + subsystem.getElevator().setAction(elevatorGotoAction); state = "Goto"; }else{ - elevatorThreshAction.start(); + subsystem.getElevator().setAction(elevatorThreshAction); state = "Thresh"; } } @@ -67,7 +67,7 @@ public void run() throws Exception{ } // If the elevator has moved up past the elevator threshold, move the wrist to its target position if (elevatorThreshAction.isDone() && state.equals("Thresh")){ - wristAction.start(); + subsystem.getWrist().setAction(wristAction); state = "WristGoto"; } @@ -77,7 +77,7 @@ public void run() throws Exception{ return; } if(state.equals("WristGoto")){ - elevatorGotoAction.start(); + subsystem.getElevator().setAction(elevatorGotoAction); state = "Goto"; } } From 94c986b58f29461c6e4e88566db73748237d6b82 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Mon, 12 Feb 2024 18:13:45 -0800 Subject: [PATCH 3/8] updated to support params value settings as well --- .../java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index 57187af..771fd5b 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -14,6 +14,9 @@ public class AmpTrapGotoAction extends Action { private double wristTarget; private String state; + public AmpTrapGotoAction(AmpTrapSubsystem subsystem, String elevatorTarget, String wristTarget) throws Exception{ + this(subsystem, subsystem.getSettingsValue(elevatorTarget).getDouble(), subsystem.getSettingsValue(wristTarget).getDouble()); + } public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, double wristTarget) throws Exception{ super(subsystem.getRobot().getMessageLogger()); this.subsystem = subsystem; From 2665dc1c545863f48b82c82508ba237747c21f05 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Mon, 12 Feb 2024 20:04:51 -0800 Subject: [PATCH 4/8] created the no-go-zones for the wrist in the params, and the elevator threshold --- src/main/deploy/allegro2024.jsonc | 7 ++++++- .../frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java | 6 +++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/allegro2024.jsonc b/src/main/deploy/allegro2024.jsonc index 11208f1..2c62d02 100755 --- a/src/main/deploy/allegro2024.jsonc +++ b/src/main/deploy/allegro2024.jsonc @@ -70,6 +70,7 @@ "ampprep" : 2, "trapprep" : 3 }, + "threshold" : 0.75, "pids" : { "position" : { "kp" : 0.0035, @@ -115,6 +116,10 @@ "ampprep" : 225, "trapprep" : 135 }, + "no-go-zone" : { + "upper" : 90, + "lower" : 0 + }, "pids" : { "position" : { "kp" : 0.0035, @@ -553,7 +558,7 @@ "current-limit" : 20.0, "p" : 0.002346041055718470, "i" : 0.0, - "d" : 0.000001172851562500, + "d" : 0.000001172851562500 }, "drive" : { "type": "talon-fx", diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index 771fd5b..dd9e882 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -22,9 +22,9 @@ public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, doub this.subsystem = subsystem; // Get the elevator threshold and the no-go zone limits for the wrist from the params file - this.elevatorThreshold = subsystem.getSettingsValue("elevatorThreshold").getDouble(); - this.wristNoGoZoneLower = subsystem.getSettingsValue("wristNoGoZoneLower").getDouble(); - this.wristNoGoZoneUpper = subsystem.getSettingsValue("wristNoGoZoneUpper").getDouble(); + this.elevatorThreshold = subsystem.getElevator().getSettingsValue("threshold").getDouble(); + this.wristNoGoZoneLower = subsystem.getArm().getSettingsValue("no-go-zone:lower").getDouble(); + this.wristNoGoZoneUpper = subsystem.getSettingsValue("no-go-zone:upper").getDouble(); this.wristTarget = wristTarget; From 7769c7c4fd15f1743a5d8fe93282dd4ee182ca63 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Mon, 12 Feb 2024 20:36:18 -0800 Subject: [PATCH 5/8] got arid of the wrist --- src/main/deploy/allegro2024.jsonc | 45 -------------- .../robot/automodes/AllegroTestAutoMode.java | 58 +++++++++---------- .../amp_trap/AmpTrapEjectAction.java | 8 +-- .../amp_trap/AmpTrapGotoAction.java | 50 ++++++++-------- .../amp_trap/AmpTrapStowAction.java | 8 +-- .../subsystems/amp_trap/AmpTrapSubsystem.java | 14 ++--- .../subsystems/amp_trap/PrepAmpAction.java | 10 ++-- .../subsystems/amp_trap/PrepClimbAction.java | 8 +-- .../subsystems/amp_trap/PrepTrapAction.java | 10 ++-- 9 files changed, 83 insertions(+), 128 deletions(-) diff --git a/src/main/deploy/allegro2024.jsonc b/src/main/deploy/allegro2024.jsonc index 2c62d02..43b052a 100755 --- a/src/main/deploy/allegro2024.jsonc +++ b/src/main/deploy/allegro2024.jsonc @@ -136,51 +136,6 @@ } } }, - "wrist" : { - "hw" : { - "motors" : { - "type" : "talon-fx", - "bus" : "", - "canid" : 8, - "inverted" : false, - "neutral-mode" : "brake", - "current-limit" : 60 - }, - "encoder" : { - "type" : "motor", - "m" : 0.17578125, - "b" : 0, - "posunits" : "deg", - "velunits" : "deg/s", - "accunits" : "deg/s/s" - } - }, - "props" : { - "position-important" : "high", - "velocity-important" : "high" - }, - "targets" : { - "stow" : 0, - "transfer" : -90, - "trapprep" : 0, - "ampprep" : -30 - }, - "pids" : { - "position" : { - "kp" : 0.0035, - "ki" : 0.0, - "kd" : 0.004, - "kv" : 0.000312, - "ka" : 0.0, - "kg" : 0.0, - "ks" : 0.0, - "outmax" : 12.0, - "maxv" : 270.0, - "maxa" : 270.0, - "jerk" : 0.0 - } - } - }, "manipulator" : { "hw" : { "motors" : { diff --git a/src/main/java/frc/robot/automodes/AllegroTestAutoMode.java b/src/main/java/frc/robot/automodes/AllegroTestAutoMode.java index 6e686df..7a25fd2 100755 --- a/src/main/java/frc/robot/automodes/AllegroTestAutoMode.java +++ b/src/main/java/frc/robot/automodes/AllegroTestAutoMode.java @@ -288,35 +288,35 @@ public AllegroTestAutoMode(AutoController ctrl) throws Exception { } break; - ///////////////////////////////////////////////////////////////////////// - // - // Wrists motor tests - // - ///////////////////////////////////////////////////////////////////////// - case 80: - if (amptrap != null && amptrap.getWrist() != null) { - addSubActionPair(amptrap.getWrist(), - new MotorEncoderPowerAction(amptrap.getWrist(), getDouble("power"), getDouble("duration")), - true); - } - break; - - case 81: - if (amptrap != null && amptrap.getWrist() != null) { - double duration = getDouble("duration"); - double[] times = new double[] { duration, duration, duration, duration, duration }; - double[] powers = new double[] { 0.1, 0.3, 0.5, 0.7, 0.9 }; - addSubActionPair(amptrap.getWrist(), - new MotorPowerSequenceAction(amptrap.getWrist(), times, powers), true); - } - break; - - case 82: - if (amptrap != null && amptrap.getWrist() != null) { - addSubActionPair(amptrap.getWrist(), - new MCVelocityAction(amptrap.getWrist(), "pids:velocity", getDouble("velocity"), true), true); - } - break; + // ///////////////////////////////////////////////////////////////////////// + // // + // // Wrists motor tests + // // + // ///////////////////////////////////////////////////////////////////////// + // case 80: + // if (amptrap != null && amptrap.getWrist() != null) { + // addSubActionPair(amptrap.getWrist(), + // new MotorEncoderPowerAction(amptrap.getWrist(), getDouble("power"), getDouble("duration")), + // true); + // } + // break; + + // case 81: + // if (amptrap != null && amptrap.getWrist() != null) { + // double duration = getDouble("duration"); + // double[] times = new double[] { duration, duration, duration, duration, duration }; + // double[] powers = new double[] { 0.1, 0.3, 0.5, 0.7, 0.9 }; + // addSubActionPair(amptrap.getWrist(), + // new MotorPowerSequenceAction(amptrap.getWrist(), times, powers), true); + // } + // break; + + // case 82: + // if (amptrap != null && amptrap.getWrist() != null) { + // addSubActionPair(amptrap.getWrist(), + // new MCVelocityAction(amptrap.getWrist(), "pids:velocity", getDouble("velocity"), true), true); + // } + // break; ///////////////////////////////////////////////////////////////////////// // diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapEjectAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapEjectAction.java index a000607..6c366df 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapEjectAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapEjectAction.java @@ -20,7 +20,7 @@ private enum State { private AmpTrapSubsystem sub_; private MCMotionMagicAction eject_elevator_; private MCMotionMagicAction eject_arm_; - private MCMotionMagicAction eject_wrist_; + //private MCMotionMagicAction eject_wrist_; private MCVelocityAction eject_manipulator_; private XeroTimer timer_; private State state_; @@ -36,7 +36,7 @@ public AmpTrapEjectAction(AmpTrapSubsystem sub) throws Exception { sub_ = sub; eject_elevator_ = new MCMotionMagicAction(sub_.getElevator(), "pids:position" , "targets:stow" , 0.5 , 1); eject_arm_ = new MCMotionMagicAction(sub_.getArm(), "pids:position" , "targets:stow" , 0.5 , 1); - eject_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); + //eject_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); eject_manipulator_ = new MCVelocityAction(sub_.getManipulator(), "pids:position", "targets:eject", true); timer_ = new XeroTimer(sub_.getRobot(), "Eject", 1.5); stow_amp_trap_ = new AmpTrapStowAction(sub_); @@ -48,7 +48,7 @@ public void start() throws Exception{ state_ = State.ReadyToEject; sub_.getElevator().setAction(eject_elevator_, true); - sub_.getWrist().setAction(eject_wrist_, true); + //sub_.getWrist().setAction(eject_wrist_, true); sub_.getArm().setAction(eject_arm_, true); } @@ -81,7 +81,7 @@ public String toString(int indent) { // Creating and defining what happens during each state before it can switch private void stateReadyToEject(){ - if (eject_elevator_.isDone() && eject_arm_.isDone() && eject_wrist_.isDone()){ + if (eject_elevator_.isDone() && eject_arm_.isDone()){ state_ = State.Eject; sub_.getManipulator().setAction(eject_manipulator_, true); timer_.start(); diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index dd9e882..85313cb 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -6,33 +6,33 @@ public class AmpTrapGotoAction extends Action { private MCMotionMagicAction elevatorThreshAction; private MCMotionMagicAction elevatorGotoAction; - private MCMotionMagicAction wristAction; + private MCMotionMagicAction armAction; private AmpTrapSubsystem subsystem; private double elevatorThreshold; - private double wristNoGoZoneLower; - private double wristNoGoZoneUpper; - private double wristTarget; + private double armNoGoZoneLower; + private double armNoGoZoneUpper; + private double armTarget; private String state; - public AmpTrapGotoAction(AmpTrapSubsystem subsystem, String elevatorTarget, String wristTarget) throws Exception{ - this(subsystem, subsystem.getSettingsValue(elevatorTarget).getDouble(), subsystem.getSettingsValue(wristTarget).getDouble()); + public AmpTrapGotoAction(AmpTrapSubsystem subsystem, String elevatorTarget, String armTarget) throws Exception{ + this(subsystem, subsystem.getSettingsValue(elevatorTarget).getDouble(), subsystem.getSettingsValue(armTarget).getDouble()); } - public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, double wristTarget) throws Exception{ + public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, double armTarget) throws Exception{ super(subsystem.getRobot().getMessageLogger()); this.subsystem = subsystem; - // Get the elevator threshold and the no-go zone limits for the wrist from the params file + // Get the elevator threshold and the no-go zone limits for the arm from the params file this.elevatorThreshold = subsystem.getElevator().getSettingsValue("threshold").getDouble(); - this.wristNoGoZoneLower = subsystem.getArm().getSettingsValue("no-go-zone:lower").getDouble(); - this.wristNoGoZoneUpper = subsystem.getSettingsValue("no-go-zone:upper").getDouble(); + this.armNoGoZoneLower = subsystem.getArm().getSettingsValue("no-go-zone:lower").getDouble(); + this.armNoGoZoneUpper = subsystem.getSettingsValue("no-go-zone:upper").getDouble(); - this.wristTarget = wristTarget; + this.armTarget = armTarget; this.elevatorThreshAction = new MCMotionMagicAction(subsystem.getElevator(), "ElevatorThresh", elevatorThreshold, 0.01, 0.02); - this.elevatorGotoAction = new MCMotionMagicAction(subsystem.getWrist(), "ElevatorThresh", elevatorTarget, 0.01, 0.02); + this.elevatorGotoAction = new MCMotionMagicAction(subsystem.getArm(), "ElevatorThresh", elevatorTarget, 0.01, 0.02); - // Create the MCMotionMagicAction for the wrist motor - this.wristAction = new MCMotionMagicAction(subsystem.getWrist(), "WristGoto", wristTarget, 3, 10); + // Create the MCMotionMagicAction for the arm motor + this.armAction = new MCMotionMagicAction(subsystem.getArm(), "ArmGoto", armTarget, 3, 10); state = "Thresh"; } @@ -42,11 +42,11 @@ public void start() throws Exception{ super.start(); subsystem.getElevator().setAction(elevatorGotoAction); subsystem.getElevator().setAction(elevatorThreshAction); - subsystem.getWrist().setAction(wristAction); + subsystem.getArm().setAction(armAction); // Start the MCMotionMagicActions - if(wristTarget < wristNoGoZoneLower && wristTarget > wristNoGoZoneUpper){ - subsystem.getWrist().setAction(wristAction); + if(armTarget < armNoGoZoneLower && armTarget > armNoGoZoneUpper){ + subsystem.getArm().setAction(armAction); subsystem.getElevator().setAction(elevatorGotoAction); state = "Goto"; }else{ @@ -62,24 +62,24 @@ public void run() throws Exception{ if(state.equals("Goto")){ elevatorGotoAction.run(); } - if(state.equals("Goto") || state.equals("WristGoto")){ - wristAction.run(); + if(state.equals("Goto") || state.equals("ArmGoto")){ + armAction.run(); } if(state.equals("Thresh")){ elevatorThreshAction.run(); } - // If the elevator has moved up past the elevator threshold, move the wrist to its target position + // If the elevator has moved up past the elevator threshold, move the arm to its target position if (elevatorThreshAction.isDone() && state.equals("Thresh")){ - subsystem.getWrist().setAction(wristAction); - state = "WristGoto"; + subsystem.getArm().setAction(armAction); + state = "ArmGoto"; } - if(wristAction.isDone()){ + if(armAction.isDone()){ if(elevatorGotoAction.isDone() && state.equals("Goto")){ setDone(); return; } - if(state.equals("WristGoto")){ + if(state.equals("ArmGoto")){ subsystem.getElevator().setAction(elevatorGotoAction); state = "Goto"; } @@ -92,7 +92,7 @@ public void cancel() { // Cancel the MCMotionMagicActions elevatorGotoAction.cancel(); elevatorThreshAction.cancel(); - wristAction.cancel(); + armAction.cancel(); } @Override diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapStowAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapStowAction.java index 964f68a..fc05740 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapStowAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapStowAction.java @@ -14,7 +14,7 @@ public class AmpTrapStowAction extends Action{ private MCMotionMagicAction stow_elevator_; private MotorEncoderPowerAction stow_roller_; private MCMotionMagicAction stow_pivot_; - private MCMotionMagicAction stow_wrist_; + //private MCMotionMagicAction stow_wrist_; private MCMotionMagicAction stow_climber_; @@ -26,7 +26,7 @@ public AmpTrapStowAction(AmpTrapSubsystem sub) throws Exception { stow_elevator_ = new MCMotionMagicAction(sub_.getElevator(), "pids:position" , "targets:stow" , 0.5 , 1); stow_roller_ = new MotorEncoderPowerAction(sub_.getManipulator(), 0); stow_pivot_ = new MCMotionMagicAction(sub_.getArm(), "pids:position" , "targets:stow" , 0.5 , 1); - stow_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); + //stow_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); stow_climber_ = new MCMotionMagicAction(sub_.getClimber(), "pids:position", "targets:stow", 0.5, 1); } @@ -37,7 +37,7 @@ public void start() throws Exception{ sub_.getElevator().setAction(stow_elevator_, true); sub_.getManipulator().setAction(stow_roller_, true); sub_.getArm().setAction(stow_pivot_, true); - sub_.getWrist().setAction(stow_wrist_, true); + //sub_.getWrist().setAction(stow_wrist_, true); sub_.getClimber().setAction(stow_climber_, true); } @@ -45,7 +45,7 @@ public void start() throws Exception{ public void run() throws Exception { super.run() ; - if (stow_elevator_.isDone() && stow_pivot_.isDone() && stow_wrist_.isDone() && stow_climber_.isDone()) { + if (stow_elevator_.isDone() && stow_pivot_.isDone() && stow_climber_.isDone()) { setDone(); } diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapSubsystem.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapSubsystem.java index 30fb53c..139472c 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapSubsystem.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapSubsystem.java @@ -10,8 +10,8 @@ public class AmpTrapSubsystem extends Subsystem { // The arm is attached to the top of the elevator, and moves the manipulator and wrist. 0 is fully pointed down, and it is calculated in degrees. private MotorEncoderSubsystem arm_; - // The wrist is attached to the end of the arm. It has the manipulator attached to it. 0 is fully pointed down (the manipulator wheels are paralel with the elevator), and it is calculated in degrees. - private MotorEncoderSubsystem wrist_; + // // The wrist is attached to the end of the arm. It has the manipulator attached to it. 0 is fully pointed down (the manipulator wheels are paralel with the elevator), and it is calculated in degrees. + // private MotorEncoderSubsystem wrist_; // The manipulator is the two rolling wheels attached to the wrist. They are used for holding and placing the note. They should use a power action, and it is calculated in revolutions. private MotorEncoderSubsystem manipulator_; @@ -28,8 +28,8 @@ public AmpTrapSubsystem(Subsystem parent) throws Exception { arm_ = new MotorEncoderSubsystem(this, "arm", true); addChild(arm_); - wrist_ = new MotorEncoderSubsystem(this, "wrist", true); - addChild(wrist_); + // wrist_ = new MotorEncoderSubsystem(this, "wrist", true); + // addChild(wrist_); manipulator_ = new MotorEncoderSubsystem(this, "manipulator", false); addChild(manipulator_); @@ -46,9 +46,9 @@ public MotorEncoderSubsystem getArm() { return arm_; } - public MotorEncoderSubsystem getWrist() { - return wrist_; - } + // public MotorEncoderSubsystem getWrist() { + // return wrist_; + // } public MotorEncoderSubsystem getManipulator() { return manipulator_; diff --git a/src/main/java/frc/robot/subsystems/amp_trap/PrepAmpAction.java b/src/main/java/frc/robot/subsystems/amp_trap/PrepAmpAction.java index 1082ceb..8063216 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/PrepAmpAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/PrepAmpAction.java @@ -10,8 +10,8 @@ public class PrepAmpAction extends Action{ // Moves arm private MCMotionMagicAction tiltArm_; - // Moves wrist - private MCMotionMagicAction tiltWrist_; + // // Moves wrist + // private MCMotionMagicAction tiltWrist_; private AmpTrapSubsystem sub_; @@ -21,7 +21,7 @@ public PrepAmpAction (AmpTrapSubsystem sub) throws Exception{ sub_ = sub; elevate_ = new MCMotionMagicAction(sub.getElevator(), "pids:position", "targets:ampprep", 0.5, 0.5); tiltArm_ = new MCMotionMagicAction(sub.getArm(), "pids:position", "targets:ampprep", 1, 1); - tiltWrist_ = new MCMotionMagicAction(sub.getWrist(), "pids:position", "targets:ampprep", 2, 2); + //tiltWrist_ = new MCMotionMagicAction(sub.getWrist(), "pids:position", "targets:ampprep", 2, 2); } @Override @@ -29,13 +29,13 @@ public void start() throws Exception{ super.start(); sub_.getElevator().setAction(elevate_, true); sub_.getArm().setAction(tiltArm_, true); - sub_.getWrist().setAction(tiltWrist_, true); + //sub_.getWrist().setAction(tiltWrist_, true); } @Override public void run() throws Exception{ super.run(); - if(elevate_.isDone() && tiltArm_.isDone() && tiltWrist_.isDone()){ + if(elevate_.isDone() && tiltArm_.isDone()){ setDone(); } } diff --git a/src/main/java/frc/robot/subsystems/amp_trap/PrepClimbAction.java b/src/main/java/frc/robot/subsystems/amp_trap/PrepClimbAction.java index e28f6f1..db92f7d 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/PrepClimbAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/PrepClimbAction.java @@ -10,7 +10,7 @@ public class PrepClimbAction extends Action{ private MCMotionMagicAction stow_elevator_; private MotorEncoderPowerAction stow_roller_; private MCMotionMagicAction stow_pivot_; - private MCMotionMagicAction stow_wrist_; + //private MCMotionMagicAction stow_wrist_; public PrepClimbAction(AmpTrapSubsystem sub) throws Exception{ super(sub.getRobot().getMessageLogger()); sub_ = sub; @@ -18,7 +18,7 @@ public PrepClimbAction(AmpTrapSubsystem sub) throws Exception{ stow_elevator_ = new MCMotionMagicAction(sub_.getElevator(), "pids:position" , "targets:stow" , 0.5 , 1); stow_roller_ = new MotorEncoderPowerAction(sub_.getManipulator(), 0); stow_pivot_ = new MCMotionMagicAction(sub_.getArm(), "pids:position" , "targets:stow" , 0.5 , 1); - stow_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); + //stow_wrist_ = new MCMotionMagicAction(sub_.getWrist(), "pids:position" , "targets:stow" , 0.5 , 1); } @Override @@ -27,14 +27,14 @@ public void start() throws Exception{ sub_.getElevator().setAction(stow_elevator_, true); sub_.getManipulator().setAction(stow_roller_, true); sub_.getArm().setAction(stow_pivot_, true); - sub_.getWrist().setAction(stow_wrist_, true); + //sub_.getWrist().setAction(stow_wrist_, true); sub_.getClimber().setAction(climb_, true); } @Override public void run() throws Exception{ super.run(); - if(climb_.isDone() && stow_elevator_.isDone() && stow_pivot_.isDone() && stow_wrist_.isDone()){ + if(climb_.isDone() && stow_elevator_.isDone() && stow_pivot_.isDone()){ setDone(); } } diff --git a/src/main/java/frc/robot/subsystems/amp_trap/PrepTrapAction.java b/src/main/java/frc/robot/subsystems/amp_trap/PrepTrapAction.java index 30e8ad5..57d6b7e 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/PrepTrapAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/PrepTrapAction.java @@ -10,8 +10,8 @@ public class PrepTrapAction extends Action{ // Moves arm private MCMotionMagicAction tiltArm_; - // Moves wrist - private MCMotionMagicAction tiltWrist_; + // // Moves wrist + // private MCMotionMagicAction tiltWrist_; // Extends climber private MCMotionMagicAction extendClimber_; @@ -24,7 +24,7 @@ public PrepTrapAction (AmpTrapSubsystem sub) throws Exception{ sub_ = sub; elevate_ = new MCMotionMagicAction(sub.getElevator(), "pids:position", "targets:trapprep", 0.5, 0.5); tiltArm_ = new MCMotionMagicAction(sub.getArm(), "pids:position", "targets:trapprep", 1, 1); - tiltWrist_ = new MCMotionMagicAction(sub.getWrist(), "pids:position", "targets:trapprep", 2, 2); + //tiltWrist_ = new MCMotionMagicAction(sub.getWrist(), "pids:position", "targets:trapprep", 2, 2); extendClimber_ = new MCMotionMagicAction(sub.getClimber(), "pids:position", "targets:climbprep", 0.5, 0.5); } @@ -33,14 +33,14 @@ public void start() throws Exception{ super.start(); sub_.getElevator().setAction(elevate_, true); sub_.getArm().setAction(tiltArm_, true); - sub_.getWrist().setAction(tiltWrist_, true); + //sub_.getWrist().setAction(tiltWrist_, true); sub_.getClimber().setAction(extendClimber_, true); } @Override public void run() throws Exception{ super.run(); - if(elevate_.isDone() && tiltArm_.isDone() && tiltWrist_.isDone() && extendClimber_.isDone()){ + if(elevate_.isDone() && tiltArm_.isDone() && extendClimber_.isDone()){ setDone(); } } From d624f8d9220186faae699eb1236208d09b0fdcf6 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Tue, 13 Feb 2024 18:43:34 -0800 Subject: [PATCH 6/8] Made so the goto action cant move through the no-go-zone --- src/main/deploy/allegro2024.jsonc | 4 ++-- .../java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/allegro2024.jsonc b/src/main/deploy/allegro2024.jsonc index 43b052a..c2cc42e 100755 --- a/src/main/deploy/allegro2024.jsonc +++ b/src/main/deploy/allegro2024.jsonc @@ -117,8 +117,8 @@ "trapprep" : 135 }, "no-go-zone" : { - "upper" : 90, - "lower" : 0 + "upper" : 70, + "lower" : 10 }, "pids" : { "position" : { diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index 85313cb..26abac2 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -45,7 +45,7 @@ public void start() throws Exception{ subsystem.getArm().setAction(armAction); // Start the MCMotionMagicActions - if(armTarget < armNoGoZoneLower && armTarget > armNoGoZoneUpper){ + if(((armTarget < armNoGoZoneLower && armTarget > armNoGoZoneUpper) && !(armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() < armNoGoZoneLower)) || ((armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() > armNoGoZoneUpper) || (armTarget < armNoGoZoneLower && subsystem.getArm().getPosition() < armNoGoZoneLower))){ subsystem.getArm().setAction(armAction); subsystem.getElevator().setAction(elevatorGotoAction); state = "Goto"; From da608a023f03f1167e2cc4deb0959cc0d29a1d12 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Wed, 14 Feb 2024 14:54:35 -0800 Subject: [PATCH 7/8] Updated to meet butch's requirements, and made it faster supposedly --- .../amp_trap/AmpTrapGotoAction.java | 51 ++++++++++++++----- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index 26abac2..ae784cd 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -3,6 +3,8 @@ import org.xero1425.base.actions.Action; import org.xero1425.base.subsystems.motorsubsystem.MCMotionMagicAction; + + public class AmpTrapGotoAction extends Action { private MCMotionMagicAction elevatorThreshAction; private MCMotionMagicAction elevatorGotoAction; @@ -12,7 +14,15 @@ public class AmpTrapGotoAction extends Action { private double armNoGoZoneLower; private double armNoGoZoneUpper; private double armTarget; - private String state; + private double elevatorTarget; + private enum State { + Goto, + ThreshGoto, + ArmGoto, + Thresh + }; + + private State state; public AmpTrapGotoAction(AmpTrapSubsystem subsystem, String elevatorTarget, String armTarget) throws Exception{ this(subsystem, subsystem.getSettingsValue(elevatorTarget).getDouble(), subsystem.getSettingsValue(armTarget).getDouble()); @@ -24,9 +34,10 @@ public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, doub // Get the elevator threshold and the no-go zone limits for the arm from the params file this.elevatorThreshold = subsystem.getElevator().getSettingsValue("threshold").getDouble(); this.armNoGoZoneLower = subsystem.getArm().getSettingsValue("no-go-zone:lower").getDouble(); - this.armNoGoZoneUpper = subsystem.getSettingsValue("no-go-zone:upper").getDouble(); + this.armNoGoZoneUpper = subsystem.getArm().getSettingsValue("no-go-zone:upper").getDouble(); this.armTarget = armTarget; + this.elevatorTarget = elevatorTarget; this.elevatorThreshAction = new MCMotionMagicAction(subsystem.getElevator(), "ElevatorThresh", elevatorThreshold, 0.01, 0.02); this.elevatorGotoAction = new MCMotionMagicAction(subsystem.getArm(), "ElevatorThresh", elevatorTarget, 0.01, 0.02); @@ -34,7 +45,7 @@ public AmpTrapGotoAction(AmpTrapSubsystem subsystem, double elevatorTarget, doub // Create the MCMotionMagicAction for the arm motor this.armAction = new MCMotionMagicAction(subsystem.getArm(), "ArmGoto", armTarget, 3, 10); - state = "Thresh"; + state = State.Thresh; } @Override @@ -45,13 +56,20 @@ public void start() throws Exception{ subsystem.getArm().setAction(armAction); // Start the MCMotionMagicActions - if(((armTarget < armNoGoZoneLower && armTarget > armNoGoZoneUpper) && !(armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() < armNoGoZoneLower)) || ((armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() > armNoGoZoneUpper) || (armTarget < armNoGoZoneLower && subsystem.getArm().getPosition() < armNoGoZoneLower))){ + // If arm target isn't in no-go-zone + // and the arm target and arm actual aren't on opposite sides + // then they can be run in paralell + if(((armTarget < armNoGoZoneLower || armTarget > armNoGoZoneUpper) && (!(armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() < armNoGoZoneLower) || !(armTarget < armNoGoZoneLower && subsystem.getArm().getPosition() > armNoGoZoneUpper)))){ subsystem.getArm().setAction(armAction); subsystem.getElevator().setAction(elevatorGotoAction); - state = "Goto"; + state = State.Goto; + }else if (elevatorTarget > elevatorThreshold){ + // Otherwise, start the + subsystem.getElevator().setAction(elevatorGotoAction); + state = State.ThreshGoto; }else{ subsystem.getElevator().setAction(elevatorThreshAction); - state = "Thresh"; + state = State.Thresh; } } @@ -59,29 +77,34 @@ public void start() throws Exception{ public void run() throws Exception{ super.run(); // Update the MCMotionMagicActions - if(state.equals("Goto")){ + if(state == State.Goto || state == State.ThreshGoto){ elevatorGotoAction.run(); } - if(state.equals("Goto") || state.equals("ArmGoto")){ + if(state == State.Goto || state == State.ArmGoto){ armAction.run(); } - if(state.equals("Thresh")){ + if(state == State.Thresh){ elevatorThreshAction.run(); } + + if(state == State.ThreshGoto && subsystem.getElevator().getPosition() > elevatorThreshold){ + subsystem.getArm().setAction(armAction); + state = State.Goto; + } // If the elevator has moved up past the elevator threshold, move the arm to its target position - if (elevatorThreshAction.isDone() && state.equals("Thresh")){ + if (elevatorThreshAction.isDone() && state == State.Thresh){ subsystem.getArm().setAction(armAction); - state = "ArmGoto"; + state = State.ArmGoto; } if(armAction.isDone()){ - if(elevatorGotoAction.isDone() && state.equals("Goto")){ + if(elevatorGotoAction.isDone() && state == State.Goto){ setDone(); return; } - if(state.equals("ArmGoto")){ + if(state == State.ArmGoto){ subsystem.getElevator().setAction(elevatorGotoAction); - state = "Goto"; + state = State.Goto; } } } From 9955523915df6bf7da5bc8f85b7537a941958b27 Mon Sep 17 00:00:00 2001 From: Michael <107226999+themehdev@users.noreply.github.com> Date: Fri, 16 Feb 2024 13:51:22 -0800 Subject: [PATCH 8/8] hopefully that fixes the problem --- .../amp_trap/AmpTrapGotoAction.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java index ae784cd..f02c03b 100644 --- a/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -58,17 +58,17 @@ public void start() throws Exception{ // Start the MCMotionMagicActions // If arm target isn't in no-go-zone // and the arm target and arm actual aren't on opposite sides - // then they can be run in paralell - if(((armTarget < armNoGoZoneLower || armTarget > armNoGoZoneUpper) && (!(armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() < armNoGoZoneLower) || !(armTarget < armNoGoZoneLower && subsystem.getArm().getPosition() > armNoGoZoneUpper)))){ - subsystem.getArm().setAction(armAction); - subsystem.getElevator().setAction(elevatorGotoAction); + // then they can be run in paralel + if(((armTarget < armNoGoZoneLower || armTarget > armNoGoZoneUpper) && !(armTarget > armNoGoZoneUpper && subsystem.getArm().getPosition() < armNoGoZoneLower) && !(armTarget < armNoGoZoneLower && subsystem.getArm().getPosition() > armNoGoZoneUpper))){ + subsystem.getArm().setAction(armAction, true); + subsystem.getElevator().setAction(elevatorGotoAction, true); state = State.Goto; }else if (elevatorTarget > elevatorThreshold){ - // Otherwise, start the - subsystem.getElevator().setAction(elevatorGotoAction); + // Otherwise, start the actions + subsystem.getElevator().setAction(elevatorGotoAction, true); state = State.ThreshGoto; }else{ - subsystem.getElevator().setAction(elevatorThreshAction); + subsystem.getElevator().setAction(elevatorThreshAction, true); state = State.Thresh; } } @@ -88,12 +88,12 @@ public void run() throws Exception{ } if(state == State.ThreshGoto && subsystem.getElevator().getPosition() > elevatorThreshold){ - subsystem.getArm().setAction(armAction); + subsystem.getArm().setAction(armAction, true); state = State.Goto; } // If the elevator has moved up past the elevator threshold, move the arm to its target position if (elevatorThreshAction.isDone() && state == State.Thresh){ - subsystem.getArm().setAction(armAction); + subsystem.getArm().setAction(armAction, true); state = State.ArmGoto; } @@ -103,7 +103,7 @@ public void run() throws Exception{ return; } if(state == State.ArmGoto){ - subsystem.getElevator().setAction(elevatorGotoAction); + subsystem.getElevator().setAction(elevatorGotoAction, true); state = State.Goto; } }