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

Version 4 (modified by David Albert, 6 years ago) (diff)

--

Modify the generated code as follows:

  • Add the following imports:
    import com.ctre.phoenix.motorcontrol.can.*;
    import com.ctre.phoneix.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();
      // Quadrature encoders are connected directly to the motor controllers
      leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
      rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
      // Reset encoder counts to 0
      leftMotor.setSelectedSensorPosition(0);
      rightMotor.setSelectedSensorPosition(0);
    
  • 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:
      // 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);
    
  • 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