Modify the generated code as follows: * Add the following imports: {{{ import edu.wpi.first.wpilibj.Encoder; import com.ctre.phoenix.motorcontrol.can.*; // for Hazelnut import edu.wpi.first.wpilibj.Talon; // for Macadamia }}} * Declare variables for the Encoders and Motor controllers in the Robot class {{{ // Uncomment next 2 lines for PWM motor controllers (Macademia) // private Talon leftMotor, rightMotor; // 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 * 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); // Peanut wheels are 7.5" in diameter // Encoders provide 1 count per degree of rotation 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); }}} * 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); }}} * For PWM controller robots (Macadamia),[[BR]] in robotPeriodic() display the encoder values: {{{ SmartDashboard.putNumber("Left", leftEncoder.getDistance()); SmartDashboard.putNumber("Right", rightEncoder.getDistance()); }}} * For CAN motor controllers (Hazelnut): * First 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 [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: {{{ // 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() reset the encoder counts and start the motors: {{{ // 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); // 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 * For PWM motor controllers: {{{ if ((leftEncoder.getDistance() > 36.0) || (rightEncoder.getDistance() > 36.0)) { leftMotor.stopMotor(); rightMotor.stopMotor(); } }}} * For CAN motor controllers: {{{ 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(); } }}}