sim working

simgui.json

@@ -30,5 +30,12 @@
         }
       }
     }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "open": true
+      }
+    }
   }
 }

src/main/java/frc/robot/Robot.java

@@ -5,7 +5,12 @@
 package frc.robot;
 
 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;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Superstructure.Position;
 import frc.robot.auto.AutoSelector;
@@ -46,7 +51,7 @@ public void robotInit() {
         drive_.setDefaultCommand(new DriveTeleop(drive_, robot_state_, driver_controller_));
 
         setupTeleopControls();
-        superstructure_.setPosition(Position.STOW);
+        // superstructure_.setPosition(Position.STOW);
     }
 
     @Override
@@ -56,10 +61,17 @@ public void robotPeriodic() {
         telemetry_.periodic();
     }
 
+    public Command test() {
+        return new SequentialCommandGroup(
+            new InstantCommand(()-> superstructure_.setPosition(Position.STOW)),
+            new WaitCommand(5.0),
+            superstructure_.setPosition(Position.CUBE_L1)
+        );
+    }
+
     @Override
     public void autonomousInit() {
         // superstructure_.setPosition(Position.CUBE_L1);
-        auto_selector_.run(robot_state_, superstructure_, arm_);
         System.out.println("AUTO ENABLED");
     }
 
@@ -68,10 +80,17 @@ public void autonomousPeriodic() {}
 
     @Override
     public void teleopInit() {
+        test().schedule();
     }
 
     @Override
-    public void teleopPeriodic() {}
+    public void teleopPeriodic() {
+
+        System.out.println(superstructure_.getState());
+        // System.out.println(arm_.getAngle());
+        SmartDashboard.putNumber("Arm Angle", arm_.getAngle());
+
+    }
 
     @Override
     public void disabledInit() {}

src/main/java/frc/robot/Superstructure.java

@@ -19,7 +19,9 @@ public Superstructure(Arm arm) {
 
 
     public Command setPosition(Position pos) {
+        System.out.println("x");
         return new SequentialCommandGroup(
+            new InstantCommand(() -> System.out.println("inside setposition")),
             new InstantCommand(() -> this.state = pos.posname),
             new ArmToPosition(arm_, pos.angle).withTimeout(3)
         );

src/main/java/frc/robot/Telemetry.java

@@ -72,6 +72,6 @@ public Telemetry(RobotState robot_state, Arm arm) {
     public void periodic() {
         for (Runnable fn : periodic_registry_)
             fn.run();
-        System.out.println(periodic_registry_);
+        // System.out.println(periodic_registry_);
     }
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
@@ -96,6 +97,8 @@ public void periodic() {
                 // Set simulated inputs
                 if (RobotBase.isSimulation())
                     arm_motor_sim_.getDouble("Applied Output").set(io_.demand * 12);
+                    SmartDashboard.putNumber("ARM angle", getAngle());
+                    SmartDashboard.putNumber("ARM Demand", io_.demand);
                 break;
             case ANGLE:
                 double feedback = fb_.calculate(io_.angle);