Migrated servo code to servo branch

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.Drivetrain;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -47,7 +48,8 @@ public void robotPeriodic() {
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
   public void disabledPeriodic() {}

src/main/java/frc/robot/RobotContainer.java

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -20,10 +21,11 @@
 import edu.wpi.first.wpilibj2.command.button.Button;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
- * subsystems, commands, and button mappings) should be declared here.
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
@@ -36,9 +38,12 @@ public class RobotContainer {
   // Create SmartDashboard chooser for autonomous routines
   private final SendableChooser<Command> m_chooser = new SendableChooser<>();
 
-  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
-  // that is specified when launching the wpilib-ws server on the Romi raspberry pi.
-  // By default, the following are available (listed in order from inside of the board to outside):
+  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the
+  // hardware "overlay"
+  // that is specified when launching the wpilib-ws server on the Romi raspberry
+  // pi.
+  // By default, the following are available (listed in order from inside of the
+  // board to outside):
   // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
   // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
   // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
@@ -47,7 +52,9 @@ public class RobotContainer {
   //
   // Your subsystem configuration should take the overlays into account
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -74,8 +81,28 @@ private void configureButtonBindings() {
     m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
     m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
     SmartDashboard.putData(m_chooser);
+
+    // Shuffleboard.getTab("Mannendri")
+      // .add("Set Servo Angle", m_drivetrain.m_servo.setAngle();
+
+    //   .add("Get Servo Angle", m_drivetrain.m_servo.getAngle());
+    // Shuffleboard.getTab("Servo Angle")
+    // .add("Max Speed", 1)
+    // .withWidget(BuiltInWidgets.kTextView)
+    // .withProperties(Map.of("min",0,"max",180))
+    // .getEntry();
+
+    // Shuffleboard.
+    // .add("Servoangle", m_drivetrain.m_servo.getAngle());
+    // .withWidget(BuiltInWidgets.kTextView)
+
+
   }
 
+  // private ShuffleboardComponent<SimpleWidget> withWidget(BuiltInWidgets ktextview) {
+  //   return null;
+  // }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
@@ -93,5 +120,7 @@ public Command getAutonomousCommand() {
   public Command getArcadeDriveCommand() {
     return new ArcadeDrive(
         m_drivetrain, () -> -m_controller.getRawAxis(1), () -> m_controller.getRawAxis(0));
+
   }
+
 }

src/main/java/frc/robot/commands/ArcadeDrive.java

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import java.util.function.Supplier;
 
@@ -33,12 +34,25 @@ public ArcadeDrive(
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    Drivetrain.m_servo.setAngle(45);
+//   Shuffleboard.getTab("Servo Angle")
+  //     .add("Max Angle", 180)
+  //     .withWidget(BuiltInWidgets.kTextView)
+  //     .withProperties(Map.of("min",0,"max",180))
+  //     .getEntry();
+  //   //  .add("Servoangle", m_drivetrain.m_servo.getAngle());
+
+
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
+    SmartDashboard.putNumber("Servo Angle", Drivetrain.m_servo.getAngle());
+
+
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/AutonomousDistance.java

@@ -5,8 +5,10 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
+
 public class AutonomousDistance extends SequentialCommandGroup {
   /**
    * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
@@ -15,11 +17,25 @@ public class AutonomousDistance extends SequentialCommandGroup {
    * @param drivetrain The drivetrain subsystem on which this command will run
    */
   public AutonomousDistance(Drivetrain drivetrain) {
+      Drivetrain.m_servo.setAngle(120);
+
     addCommands(
-        new DriveDistance(-0.5, 50, drivetrain)
+         // new DriveDistance(-0.5, 50, drivetrain)
         // new TurnDegrees(-0.5, 180, drivetrain),
         // new DriveDistance(-0.5, 10, drivetrain),
         // new TurnDegrees(0.5, 180, drivetrain)
-        );
+        new InstantCommand(() -> {
+
+          // drivetrain.m_servo.write(0);
+          Drivetrain.m_servo.set(0.5);
+          Drivetrain.m_servo.setAngle(120); 
+          // drivetrain.m_servo.setAngle(0);
+
+          // drivetrain.m_servo.setAngle(0);
+          //  drivetrain.m_servo.setAngle(90);
+          //  drivetrain.m_servo.setAngle(180);
+
+         }, drivetrain)      
+         );
   }
 }

src/main/java/frc/robot/commands/DriveDistance.java

@@ -13,6 +13,7 @@ public class DriveDistance extends CommandBase {
   private final double m_distance;
   private final double m_speed;
   private final PIDController m_anglePID;
+  // private final Servo m_Servo;
 
   /**
    * Creates a new DriveDistance. This command will drive your your robot for a desired distance at
@@ -36,6 +37,7 @@ public void initialize() {
     m_drive.arcadeDrive(0, 0);
     m_drive.resetEncoders();
     m_drive.resetGyro();
+    // m_drive.m_servo.setAngle(140);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/TurnDegrees.java

@@ -33,6 +33,7 @@ public void initialize() {
     // Set motors to stop, read encoder values for starting point
     m_drive.arcadeDrive(0, 0);
     m_drive.resetEncoders();
+    // m_drive.m_servo.setAngle(140);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import frc.robot.sensors.RomiGyro;
@@ -25,6 +26,25 @@ public class Drivetrain extends SubsystemBase {
   private final Encoder m_leftEncoder = new Encoder(4, 5);
   private final Encoder m_rightEncoder = new Encoder(6, 7);
 
+    // Create Servo
+  public final static Servo m_servo = new Servo(3);
+
+  // private ShuffleboardTab tab = Shuffleboard.getTab("Servos");    
+  // Shuffleboard.getTab("Servooos")
+  //   .add("Sevroanglemanndneri", m_servo.getAngle());
+
+  // private NetworkTableEntry servoAngle = 
+  //   tab.add("Servoangles", 180)
+  //     .getEntry();
+
+  // public void setAngle() {
+  //   double angle = servoAngle.getDouble(0);
+  //   System.out.println(angle);
+  //   m_servo.setAngle(angle);
+  // }
+
+
+
   // Set up the differential drive controller
   private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
 
@@ -44,6 +64,7 @@ public Drivetrain() {
 
   public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
     m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+    // m_servo.setAngle(0);
   }
 
   public void resetEncoders() {
@@ -132,6 +153,8 @@ public void resetGyro() {
 
   @Override
   public void periodic() {
+    // m_servo.setAngle(180);
     // This method will be called once per scheduler run
   }
+
 }