Changes between Version 1 and Version 2 of ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous/PWMCode

Timestamp:

Nov 12, 2019, 6:44:26 PM (6 years ago)

Author:

David Albert

Comment:

--

ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous/PWMCode

@@ -10 +10 @@
 
 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.Encoder;
@@ -22 +23 @@
  */
 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 leftMotor, rightMotor;
   private Encoder leftEncoder, rightEncoder;
@@ -31 +36 @@
   @Override
   public void robotInit() {
+    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
+    m_chooser.addOption("My Auto", kCustomAuto);
+    SmartDashboard.putData("Auto choices", m_chooser);
    // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3
    leftEncoder = new Encoder(0, 1);
-   rightEncoder = new Encoder(2, 3);
+   rightEncoder = new Encoder(2, 3, true); // right side direction is reversed
    // Peanut wheels are 7.5" in diameter
    // Encoders provide 1 count per degree of rotation
@@ -41 +49 @@
    leftMotor = new Talon(0);
    rightMotor = new Talon(1);
+   // right side must turn in opposite direction
+   rightMotor.setInverted(true);
   }
 
@@ -58 +68 @@
 
   /**
+   * 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);
     // Reset encoder counts to 0
     leftEncoder.reset();
@@ -66 +88 @@
     // start motors turning forward at 20% power
     leftMotor.set(0.20);
-    // to go forward, left motor turns clockwise, right motor counter-clockwise
-    rightMotor.set(-0.20);
+    rightMotor.set(0.20);
   }
 
@@ -75 +96 @@
   @Override
   public void autonomousPeriodic() {
-    if ((leftEncoder.getDistance() > 36.0) || 
-        (rightEncoder.getDistance() > 36.0)) {
-      leftMotor.stopMotor();
-      rightMotor.stopMotor();
-    }        
+    switch (m_autoSelected) {
+      case kCustomAuto:
+        // Put custom auto code here
+        break;
+      case kDefaultAuto:
+      default:
+       if ((leftEncoder.getDistance() > 36.0) || 
+           (rightEncoder.getDistance() > 36.0)) {
+           leftMotor.stopMotor();
+           rightMotor.stopMotor();
+        }        
+        break;
+    }
   }