diff --git a/src/main/deploy/allegro2024.jsonc b/src/main/deploy/allegro2024.jsonc index 11208f1..c2cc42e 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,50 +116,9 @@ "ampprep" : 225, "trapprep" : 135 }, - "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 - } - } - }, - "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 + "no-go-zone" : { + "upper" : 70, + "lower" : 10 }, "pids" : { "position" : { @@ -553,7 +513,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/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 new file mode 100644 index 0000000..f02c03b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/amp_trap/AmpTrapGotoAction.java @@ -0,0 +1,125 @@ +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 armAction; + private AmpTrapSubsystem subsystem; + private double elevatorThreshold; + private double armNoGoZoneLower; + private double armNoGoZoneUpper; + private double armTarget; + 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()); + } + 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 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.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); + + // Create the MCMotionMagicAction for the arm motor + this.armAction = new MCMotionMagicAction(subsystem.getArm(), "ArmGoto", armTarget, 3, 10); + + state = State.Thresh; + } + + @Override + public void start() throws Exception{ + super.start(); + subsystem.getElevator().setAction(elevatorGotoAction); + subsystem.getElevator().setAction(elevatorThreshAction); + subsystem.getArm().setAction(armAction); + + // 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 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 actions + subsystem.getElevator().setAction(elevatorGotoAction, true); + state = State.ThreshGoto; + }else{ + subsystem.getElevator().setAction(elevatorThreshAction, true); + state = State.Thresh; + } + } + + @Override + public void run() throws Exception{ + super.run(); + // Update the MCMotionMagicActions + if(state == State.Goto || state == State.ThreshGoto){ + elevatorGotoAction.run(); + } + if(state == State.Goto || state == State.ArmGoto){ + armAction.run(); + } + if(state == State.Thresh){ + elevatorThreshAction.run(); + } + + if(state == State.ThreshGoto && subsystem.getElevator().getPosition() > elevatorThreshold){ + 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, true); + state = State.ArmGoto; + } + + if(armAction.isDone()){ + if(elevatorGotoAction.isDone() && state == State.Goto){ + setDone(); + return; + } + if(state == State.ArmGoto){ + subsystem.getElevator().setAction(elevatorGotoAction, true); + state = State.Goto; + } + } + } + + @Override + public void cancel() { + super.cancel(); + // Cancel the MCMotionMagicActions + elevatorGotoAction.cancel(); + elevatorThreshAction.cancel(); + armAction.cancel(); + } + + @Override + public String toString(int indent) { + return prefix(indent) + "AmpTrapGotoAction"; + } +} 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(); } }