/** This function is called periodically during operator control */
public void teleopPeriodic() {
// The order of the update methods is important. Besides making a nice slope of periods, the
// motors and sensors have to update first
controller.update(motorManagers);
sensors.update();
shoot.update(motorManagers, controller, sensors, SSS);
arm.update(motorManagers, controller);
// System.out.println("Encoder Raw: " + sensors.getEncoderValues());
// System.out.println("Encoder Count: " + sensors.getEncoderCount());
// System.out.println("Encoder Rate of Rotation: " + sensors.getEncoderRate());
// System.out.println("Encoder Distance: " + sensors.getEncoderDistance());
// System.out.println("Degrees per Second: " + sensors.getEncoderAngularSpeed());
// System.out.println("StringPot: " + sensors.getStringPot());
System.out.println("Ultrasonic Range Finder (Inches): " + sensors.getSonicRangeInches());
// System.out.println("Servo: " + shoot.pusher.getAngle());
sensors.resetGyroAngles(controller);
System.out.println("Gyro XY: " + sensors.getGyroXYAngle());
System.out.println("Gyro Z: " + sensors.getGyroZAngle());
System.out.println("wheel Angle: " + sensors.encoder.getEncoderAngle());
SmartDashboard.putNumber("Gyro XY", sensors.getGyroXYAngle());
SmartDashboard.putNumber("Gyro Z", sensors.getGyroZAngle());
SmartDashboard.putNumber("Distance", sensors.getSonicRangeInches());
}
