Changes between Initial Version and Version 1 of ControlSystems/SoftwareTeam/Training/GettingStarted/RobotFSM1

Timestamp:

Nov 6, 2019, 1:15:30 PM (6 years ago)

Author:

David Albert

Comment:

--

ControlSystems/SoftwareTeam/Training/GettingStarted/RobotFSM1

@@ +1 @@
+You should review the Java State Machine example before exploring this code.
+
+== State Machine for PWM robot
+{{{
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.Encoder;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private static final String kDefaultAuto = "Default";
+  private static final String kCustomAuto = "My Auto";
+  private String m_autoSelected;
+  private final SendableChooser<String> m_chooser = new SendableChooser<>();
+  private Talon l_motor, r_motor;
+  private Encoder l_encoder, r_encoder;
+
+  private enum AutoState { STARTING, DRIVING, TURNING, STOPPED };
+  private AutoState state;
+  
+  private void driveForward() {
+    // to go forward, left and right motors turn in opposite directions
+    // so wheels turn in same direction
+    l_motor.set(0.20);
+    r_motor.set(-0.20);
+    // reset encoders so we can tell how far we've driven
+    l_encoder.reset();
+    r_encoder.reset();
+  }
+
+  private void turnRight() {
+    // to turn, both motors turn in the same direction
+    // so wheels turn in opposite directions
+    l_motor.set(0.20);
+    r_motor.set(0.20);
+    // reset encoders so we can tell how far we've driven
+    l_encoder.reset();
+    r_encoder.reset();
+  }
+
+  private void stop() {
+    l_motor.stopMotor();
+    r_motor.stopMotor();
+  }
+
+  /**
+   * This function is run when the robot is first started up and should be
+   * used for any initialization code.
+   */
+  @Override
+  public void robotInit() {
+    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
+    m_chooser.addOption("My Auto", kCustomAuto);
+    SmartDashboard.putData("Auto choices", m_chooser);
+    // Talon motor controllers connected to PWM ports 0,1
+    l_motor = new Talon(0);
+    r_motor = new Talon(1);
+    // Quadrature encoders connected to DIO ports 0,1 and 2,3
+    l_encoder = new Encoder(0,1);
+    r_encoder = new Encoder(2,3);
+    // 7.5" wheels with 360-pulse-per-rotation encoders
+    l_encoder.setDistancePerPulse((7.5 * 3.141592) / 360);
+    r_encoder.setDistancePerPulse((7.5 * 3.141592) / 360);
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use
+   * this for items like diagnostics that you want ran during disabled,
+   * autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+  }
+
+  /**
+   * This autonomous (along with the chooser code above) shows how to select
+   * between different autonomous modes using the dashboard. The sendable
+   * chooser code works with the Java SmartDashboard. If you prefer the
+   * LabVIEW Dashboard, remove all of the chooser code and uncomment the
+   * getString line to get the auto name from the text box below the Gyro
+   *
+   * <p>You can add additional auto modes by adding additional comparisons to
+   * the switch structure below with additional strings. If using the
+   * SendableChooser make sure to add them to the chooser code above as well.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autoSelected = m_chooser.getSelected();
+    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
+    System.out.println("Auto selected: " + m_autoSelected);
+    state = AutoState.STARTING;
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+    switch (state) {
+      case STARTING:
+        // transition to driving state
+        driveForward();
+        state = AutoState.DRIVING;
+        break;
+      case DRIVING:
+        if ((l_encoder.getDistance() >= 36.0) &&
+            (r_encoder.getDistance() >= 36.0)) {
+            // if driven desired distance, transition to turning state
+            turnRight();
+            state = AutoState.TURNING;
+        }
+        break;
+      case TURNING:
+        if ((l_encoder.getDistance() >= 3.0) && 
+            (r_encoder.getDistance() <= -3.0)) {
+            // if turned desired distance, transition to stopped state
+            stop();
+            state = AutoState.STOPPED;
+        }
+        break;
+      case STOPPED:
+        // stay in this state until we exit autonomous
+        break;
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}
+}}}