diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99b96ea..1539e4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,5 +13,6 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static double[] servoPresets={20, 90, 10, 50}; + public static final double[] servoPresets={20, 90, 10, 50}; + public static final double servoDegreesPerSecond=600; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7044e50..a3e81bc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.Rangefinder; import frc.robot.subsystems.Servo; import frc.robot.subsystems.XRPDrivetrain; @@ -31,6 +32,7 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; private SendableChooser m_chooser = new SendableChooser<>(); + private boolean showingSensorDistance = false; /** * This function is run when the robot is first started up and should be used for any @@ -78,9 +80,9 @@ public void disabledPeriodic() {} public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } + // if (m_autonomousCommand != null) { + // m_autonomousCommand.schedule(); + // } switch (m_chooser.getSelected()) { case 1: SmartDashboard.putString("What should happen: ", "The XRP should turn about 90 degrees(don't worry if it's not accurate!)"); @@ -151,7 +153,7 @@ public void autonomousInit() { } catch (NoSuchMethodException e) { // TODO Auto-generated catch block try { - ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).newInstance()).schedule(); + ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).newInstance(5)).schedule(); } catch (ClassNotFoundException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException | SecurityException e1) { // TODO Auto-generated catch block e1.printStackTrace(); @@ -178,17 +180,33 @@ public void autonomousInit() { break; case 6: SmartDashboard.putString("What should happen: ", "The XRP should move its servo to each of the presets."); - try{ - - } break; case 7: + SmartDashboard.putString("What should happen: ", "Change the mode from Auto to Teleop. You should now be able to control the XRP's servo!"); break; - case 8: + case 8: + SmartDashboard.putString("What should happen: ", "You should now see the distance the rangefinder reads!"); + showingSensorDistance=true; break; case 9: + SmartDashboard.putString("What should happen: ", "The XRP should execute the command and move until an object in front of it is less than 2 inches away."); + try { + DriveCommands.class.getMethod("driveToWall").invoke(null); + } catch (NoSuchMethodException e) { + // TODO Auto-generated catch block + try { + ((Command) Class.forName("DriveCommands$DriveToWall").getDeclaredConstructor().newInstance()).schedule(); + } catch (ClassNotFoundException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException | SecurityException e1) { + // TODO Auto-generated catch block + e1.printStackTrace(); + } + } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } break; case 10: + SmartDashboard.putString("What should happen: ", "Change the mode from Auto to Teleop. You should now be able to control the XRP using a tank drive scheme!"); break; case 11: break; @@ -200,7 +218,16 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + if(showingSensorDistance) + try { + SmartDashboard.putNumber("Rangefinder Distance", (double) Rangefinder.class.getMethod("getDistance").invoke(RobotContainer.m_rangefinder)); + } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException + | SecurityException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } @Override public void teleopInit() { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 39ba298..fe9704d 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -229,10 +229,8 @@ public static Command sequentialExampleCommand(){ //TODO: Task 9-Write a Command(function or class) that causes the XRP to drive until the distance returned by the rangefinder - //is less than 2 inches. Afterwards, test it by replacing the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote. - + //is less than 2 inches. Name it driveToWall or DriveToWall. //TODO: Task 10-Write a Sequential Command Group to move the XRP backwards 2 inches(you may need to make new Commands or alter existing Commands for this), set the arm preset to index 1, spin the XRP 360 degrees - //and move the XRP forward 3 inches while moving the arm to 0 degrees using a by Parallel Command group. Afterwards, test it by replacing - //the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote. + //and move the XRP forward 3 inches while moving the arm to 0 degrees using a by Parallel Command group. //HINT: Parallel Command Groups are written the same way as Sequential Command Groups. } diff --git a/src/main/java/frc/robot/commands/ServoCommands.java b/src/main/java/frc/robot/commands/ServoCommands.java index e7bfb5c..e107034 100644 --- a/src/main/java/frc/robot/commands/ServoCommands.java +++ b/src/main/java/frc/robot/commands/ServoCommands.java @@ -4,5 +4,9 @@ public class ServoCommands { //TODO: Task 6-Write commands to move the arm up, down, and to a specific preset. The latter should be done by taking in an integer input. //Name them servoUpCommand, servoDownCommand, and servoPresetCommand. The presets are in the constants file. + //NOTE: To make servoPresetCommand work properly with the Command system, you will need to make a non-Instant Command, one that ends when the servo's movement is complete. + //However, because the library is programmed strangely, you only get the angle you set the servo to, not the actual angle it is at during any point in time. (ie if you set the servo to move to 90 degrees from 0 degrees and you called getAngle immediately after, before any movement occurs, you simply get the value 90) + //The speed the servo moves is stored in Constants. Use it and System.currentTimeMillis() to determine how long the servo will move for, and set the end condition to be + //when the time elapsed is greater than or equal to the time the servo moves. } diff --git a/src/main/java/frc/robot/subsystems/Rangefinder.java b/src/main/java/frc/robot/subsystems/Rangefinder.java index e0344ca..695c93c 100644 --- a/src/main/java/frc/robot/subsystems/Rangefinder.java +++ b/src/main/java/frc/robot/subsystems/Rangefinder.java @@ -9,6 +9,6 @@ public class Rangefinder extends SubsystemBase{ public Rangefinder(){ m_rangefinder=new XRPRangefinder(); } - //TODO: Task 8-Write a method to get the distance returned by the sensor in inches. + //TODO: Task 8-Write a method to get the distance returned by the sensor in inches. Name it getDistance //HINT: Type 'm_rangefinder.' (without quotes) in a method body to see all of the different methods the rangefinder has. } diff --git a/src/main/java/frc/robot/subsystems/Servo.java b/src/main/java/frc/robot/subsystems/Servo.java index 599313a..a4ac731 100644 --- a/src/main/java/frc/robot/subsystems/Servo.java +++ b/src/main/java/frc/robot/subsystems/Servo.java @@ -9,7 +9,6 @@ public class Servo extends SubsystemBase { public Servo(){ // Device number 4 maps to the physical Servo 1 port on the XRP m_armServo = new XRPServo(4); - } //TODO: Task 5-Write functions to control the arm. You should be able to set the arm's angle and get the angle(double) //it is at. The functions should be called setServoAngle and getServoAngle