Version 4 (modified by 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