Changes between Version 3 and Version 4 of ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous

Timestamp:

Nov 3, 2019, 8:40:20 PM (6 years ago)

Author:

David Albert

Comment:

--

ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous

@@ -10 +10 @@
 import edu.wpi.first.wpilibj.Talon;         // for Macadamia
 }}}
+
 *  Declare variables for the Encoders and Motor controllers in the Robot class
 {{{ 
-  private Encoder leftEncoder, rightEncoder;
 
-  // Uncomment next line for Macademia
+  // Uncomment next 2 lines for PWM motor controllers (Macademia)
   // private Talon leftMotor, rightMotor;
-  // Uncomment next line for Hazelnut
+  // private Encoder leftEncoder, rightEncoder;
+
+  // Uncomment next line for CAN motor controllers (Hazelnut)
   // private WPI_TalonSRX leftMotor, rightMotor;
 }}}
+
 *  In robotInit() instantiate the Encoder and Motor objects
-  {{{
-    // Quadrature encoder has A and B channels connected to DIO0, DIO1
+  For PWM Motor Controller robots (Macadamia):
+  {{{    
+    // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3
     leftEncoder = new Encoder(0, 1);
     rightEncoder = new Encoder(2, 3);
@@ -28 +32 @@
     leftEncoder.setDistancePerPulse((3.141592*7.5)/360);
     rightEncoder.setDistancePerPulse((3.141592*7.5)/360);
+    // PWM motor controllers connected to PWM0, PWM1
+    leftMotor = new Talon(0);
+    rightMotor = new Talon(1);
+  }}}
 
-    // Uncomment the next two lines for Macademia
-    // leftMotor = new Talon(0);
-    // rightMotor = new Talon(1);
-
-    // uncoment the next two lines for Hazelnut
-    // leftMotor = new WPI_TalonSRX(3);
-    // rightMotor = new WPI_TalonSRX(4);
+  For CAN Motor Controller robots (Hazelnut): 
+  {{{
+    // CAN motor controllers connected to CAN bus (addrs 3,4)
+    leftMotor = new WPI_TalonSRX(3);
+    rightMotor = new WPI_TalonSRX(4);
+    // clear any previously configured settings
+    leftMotor.configFactoryDefault();
+    rightMotor.configFactoryDefault();
+    // Encoders are connected directly to the motor controllers
+    leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
+    rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
   }}}
 
 * In robotPeriodic() display the encoder values
+
+  * For PWM controller robots (Macadamia):
   {{{
     SmartDashboard.putNumber("Left", leftEncoder.getDistance());
@@ -44 +58 @@
   }}}
 
+  * For CAN motor controllers (Hazelnut) note the use of the [https://www.ctr-electronics.com/downloads/api/java/html/classcom_1_1ctre_1_1phoenix_1_1motorcontrol_1_1can_1_1_base_motor_controller.html#afb934a11682e710df123eec35aba1ba9 getSelectedFeedbackSensor()] method of the motor controller object which returns raw encoder counts.  
+  Add the following function to the Robot class:
+  {{{
+     double raw2inches(int raw_count, int counts_per_rotation, double wheel_diameter) {
+        return (raw_count / counts_per_rotation) * (3.141492 * wheel_diameter);
+  {{{
+  Add the following to the robotPeriodic() method:
+    // Encoders connected to CAN controller return raw counts
+    double l_raw = leftMotor.getSelectedSensorPosition();
+    double r_raw = rightMotor.getSelectedSensorPosition();
+    // display distances on smart dashboard
+    SmartDashboard.putNumber("Left", raw2inches(l_raw, 360*2, 7.5));
+    SmartDashboard.putNumber("Right", raw2inches(r_raw, 360*2, 7.5));
+  }}}
+
 * In autonomousInit() reset the encoder counts and start the motors:
   {{{
-    // reset encoder counts to 0
-    leftEncoder.reset();
-    rightEncoder.reset();
+    // For PWM controllers (Macadamia) uncomment the next 2 lines to reset encoder counts to 0
+    // leftEncoder.reset();
+    // rightEncoder.reset();
     // start motors turning forward at 20% power
     leftMotor.set(0.20);
@@ -57 +86 @@
 * In the default section of autonomousPeriodic() stop if we've gone the desired distance.
   Notice our first use of the logical OR operator (!||) to test whether either of two conditions is true
+  * For PWM motor controllers:
   {{{
         if ((leftEncoder.getDistance() > 36.0) || 
             (rightEncoder.getDistance() > 36.0)) {
+           leftMotor.stopMotor();
+           rightMotor.stopMotor();
+        }
+   }}}
+   * For CAN motor controllers:
+   {{{
+        double l_raw = leftMotor.getSelectedSensorPosition();
+        double r_raw = rightMotor.getSelectedSensorPosition();
+        if ((raw2inches(l_raw, 360*2, 7.5) > 36.0) || 
+            (raw2inches(r_raw, 360*2, 7.5) > 36.0)) {
            leftMotor.stopMotor();
            rightMotor.stopMotor();