wiki:ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous/CAN

Modify the generated code as follows:

  • Add the following imports:
    import com.ctre.phoenix.motorcontrol.can.*;
    import com.ctre.phoenix.motorcontrol.FeedbackDevice;
    
  • Declare variables for the Motor controllers in the Robot class
       private WPI_TalonSRX leftMotor, rightMotor;
    
  • In robotInit() instantiate the motor controller objects
      // 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();
      // Right motor must turn in opposite direction
      rightMotor.setInverted(true);
      // Quadrature encoders are connected directly to the motor controllers
      leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
      rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
    
  • Add the following new method 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; note the use of the getSelectedFeedbackSensor() method of the motor controller object which returns raw encoder counts:
       // Encoders connected to CAN controller return raw counts
       int l_raw = leftMotor.getSelectedSensorPosition();
       int 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() start the motors:
      // Reset encoder counts to 0
      leftMotor.setSelectedSensorPosition(0);
      rightMotor.setSelectedSensorPosition(0);
      // start motors turning forward at 20% power
      leftMotor.set(0.20);
      rightMotor.set(0.20);
    
  • 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
          int l_raw = leftMotor.getSelectedSensorPosition();
          int 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();
          }
    

For the complete Robot.java code, see here

Last modified 6 years ago Last modified on Nov 12, 2019, 7:57:06 PM