| 6 | | * Add the following imports: |
| 7 | | {{{ |
| 8 | | import edu.wpi.first.wpilibj.Encoder; |
| 9 | | import com.ctre.phoenix.motorcontrol.can.*; // for Hazelnut |
| 10 | | import edu.wpi.first.wpilibj.Talon; // for Macadamia |
| 11 | | }}} |
| 12 | | |
| 13 | | * Declare variables for the Encoders and Motor controllers in the Robot class |
| 14 | | {{{ |
| 15 | | |
| 16 | | // Uncomment next 2 lines for PWM motor controllers (Macademia) |
| 17 | | // private Talon leftMotor, rightMotor; |
| 18 | | // private Encoder leftEncoder, rightEncoder; |
| 19 | | |
| 20 | | // Uncomment next line for CAN motor controllers (Hazelnut) |
| 21 | | // private WPI_TalonSRX leftMotor, rightMotor; |
| 22 | | }}} |
| 23 | | |
| 24 | | * In robotInit() instantiate the Encoder and Motor objects |
| 25 | | * For PWM Motor Controller robots (Macadamia): |
| 26 | | {{{ |
| 27 | | // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3 |
| 28 | | leftEncoder = new Encoder(0, 1); |
| 29 | | rightEncoder = new Encoder(2, 3); |
| 30 | | // Peanut wheels are 7.5" in diameter |
| 31 | | // Encoders provide 1 count per degree of rotation |
| 32 | | leftEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| 33 | | rightEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| 34 | | // PWM motor controllers connected to PWM0, PWM1 |
| 35 | | leftMotor = new Talon(0); |
| 36 | | rightMotor = new Talon(1); |
| 37 | | }}} |
| 38 | | * For CAN Motor Controller robots (Hazelnut): |
| 39 | | {{{ |
| 40 | | // CAN motor controllers connected to CAN bus (addrs 3,4) |
| 41 | | leftMotor = new WPI_TalonSRX(3); |
| 42 | | rightMotor = new WPI_TalonSRX(4); |
| 43 | | // clear any previously configured settings |
| 44 | | leftMotor.configFactoryDefault(); |
| 45 | | rightMotor.configFactoryDefault(); |
| 46 | | // Encoders are connected directly to the motor controllers |
| 47 | | leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); |
| 48 | | rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); |
| 49 | | }}} |
| 50 | | |
| 51 | | * For PWM controller robots (Macadamia),[[BR]] |
| 52 | | in robotPeriodic() display the encoder values: |
| 53 | | {{{ |
| 54 | | SmartDashboard.putNumber("Left", leftEncoder.getDistance()); |
| 55 | | SmartDashboard.putNumber("Right", rightEncoder.getDistance()); |
| 56 | | }}} |
| 57 | | * For CAN motor controllers (Hazelnut): |
| 58 | | * First add the following new method to the Robot class: |
| 59 | | {{{ |
| 60 | | double raw2inches(int raw_count, int counts_per_rotation, double wheel_diameter) { |
| 61 | | return (raw_count / counts_per_rotation) * (3.141492 * wheel_diameter); |
| 62 | | } |
| 63 | | }}} |
| 64 | | * 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: |
| 65 | | {{{ |
| 66 | | // Encoders connected to CAN controller return raw counts |
| 67 | | int l_raw = leftMotor.getSelectedSensorPosition(); |
| 68 | | int r_raw = rightMotor.getSelectedSensorPosition(); |
| 69 | | // display distances on smart dashboard |
| 70 | | SmartDashboard.putNumber("Left", raw2inches(l_raw, 360*2, 7.5)); |
| 71 | | SmartDashboard.putNumber("Right", raw2inches(r_raw, 360*2, 7.5)); |
| 72 | | }}} |
| 73 | | |
| 74 | | * In autonomousInit() reset the encoder counts and start the motors: |
| 75 | | {{{ |
| 76 | | // For PWM controllers (Macadamia) uncomment the next 2 lines to reset encoder counts to 0 |
| 77 | | // leftEncoder.reset(); |
| 78 | | // rightEncoder.reset(); |
| 79 | | // start motors turning forward at 20% power |
| 80 | | leftMotor.set(0.20); |
| 81 | | // to go forward, left motor turns clockwise, right motor counter-clockwise |
| 82 | | rightMotor.set(-0.20); |
| 83 | | }}} |
| 84 | | |
| 85 | | * In the default section of autonomousPeriodic() stop if we've gone the desired distance. |
| 86 | | Notice our first use of the logical OR operator (!||) to test whether either of two conditions is true |
| 87 | | * For PWM motor controllers: |
| 88 | | {{{ |
| 89 | | if ((leftEncoder.getDistance() > 36.0) || |
| 90 | | (rightEncoder.getDistance() > 36.0)) { |
| 91 | | leftMotor.stopMotor(); |
| 92 | | rightMotor.stopMotor(); |
| 93 | | } |
| 94 | | }}} |
| 95 | | * For CAN motor controllers: |
| 96 | | {{{ |
| 97 | | int l_raw = leftMotor.getSelectedSensorPosition(); |
| 98 | | int r_raw = rightMotor.getSelectedSensorPosition(); |
| 99 | | if ((raw2inches(l_raw, 360*2, 7.5) > 36.0) || |
| 100 | | (raw2inches(r_raw, 360*2, 7.5) > 36.0)) { |
| 101 | | leftMotor.stopMotor(); |
| 102 | | rightMotor.stopMotor(); |
| 103 | | } |
| 104 | | }}} |
| | 6 | * Follow [ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous/PWM these instructions] for motors with PWM motor controllers (Macadamia). |
| | 7 | * Follow [ControlSystems/SoftwareTeam/Training/GettingStarted/Autonomous/CAN these instructions] for motors with CAN motor controllers (Hazelnut). |
