API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 25th, 2019
73
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.91 KB | None | 0 0
raw download clone embed print report
  1.  
  2. package frc.robot;
  3.  
  4. import com.ctre.phoenix.motorcontrol.ControlMode;
  5. import com.ctre.phoenix.motorcontrol.FeedbackDevice;
  6. import com.ctre.phoenix.motorcontrol.NeutralMode;
  7. import com.ctre.phoenix.motorcontrol.can.TalonSRX;
  8. import com.ctre.phoenix.motorcontrol.can.VictorSPX;
  9. import com.kauailabs.navx.frc.AHRS;
  10.  
  11. import edu.wpi.first.wpilibj.DoubleSolenoid;
  12. import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  13. import edu.wpi.first.wpilibj.GenericHID.Hand;
  14. import edu.wpi.first.wpilibj.I2C.Port;
  15. import edu.wpi.first.wpilibj.Timer;
  16. import edu.wpi.first.wpilibj.XboxController;
  17. import frc.robot.Commands.CargoSearchCmd;
  18. import frc.robot.Commands.RotateCmd;
  19.  
  20. class SWDrive extends Mechanism {
  21.  
  22. private TalonSRX leftMaster, rightMaster;
  23. private VictorSPX leftSlave, rightSlave;
  24. private DoubleSolenoid gearSolenoid;
  25. private XboxController controller;
  26. private boolean tankEnabled, setpointReached, rotateSet, antiTiltEnabled, cargoSearch;
  27. private AHRS navx;
  28. private double leftTarget, rightTarget, lacc, racc;
  29. private static SWDrive driveInstance = new SWDrive();
  30. private PIDController pid;
  31.  
  32. private SWDrive() {
  33. super();
  34. leftMaster = new TalonSRX(Constants.LEFT_MASTER_PORT);
  35. leftMaster.setNeutralMode(NeutralMode.Brake);
  36. leftMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
  37. leftMaster.setSelectedSensorPosition(0, 0, 0);
  38. leftMaster.config_kP(0, Constants.LINEAR_GAINS[0], 0);
  39. leftMaster.config_kI(0, Constants.LINEAR_GAINS[1], 0);
  40. leftMaster.config_kD(0, Constants.LINEAR_GAINS[2], 0);
  41. leftMaster.configMotionAcceleration(1000, 0);
  42. leftMaster.configMotionCruiseVelocity(5000, 0);
  43. leftMaster.config_IntegralZone(0, 200, 0);
  44. leftMaster.configClosedloopRamp(0, 256);
  45. leftMaster.configOpenloopRamp(0, 256);
  46. leftMaster.configAllowableClosedloopError(0, Constants.LINEAR_EPSILON, 0);
  47.  
  48. rightMaster = new TalonSRX(Constants.RIGHT_MASTER_PORT);
  49. rightMaster.setNeutralMode(NeutralMode.Brake);
  50. rightMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
  51. rightMaster.setSelectedSensorPosition(0, 0, 0);
  52. rightMaster.setInverted(true);
  53. rightMaster.config_kP(0, Constants.LINEAR_GAINS[0], 0);
  54. rightMaster.config_kI(0, Constants.LINEAR_GAINS[1], 0);
  55. rightMaster.config_kD(0, Constants.LINEAR_GAINS[2], 0);
  56. rightMaster.configMotionAcceleration(1000, 0);
  57. rightMaster.configMotionCruiseVelocity(5000, 0);
  58. rightMaster.config_IntegralZone(0, 200, 0);
  59. rightMaster.configClosedloopRamp(0, 256);
  60. rightMaster.configOpenloopRamp(0, 256);
  61. rightMaster.configAllowableClosedloopError(0, Constants.LINEAR_EPSILON, 0);
  62.  
  63. leftSlave = new VictorSPX(Constants.LEFT_SLAVE_PORT);
  64. leftSlave.setNeutralMode(NeutralMode.Brake);
  65. leftSlave.follow(leftMaster);
  66.  
  67. rightSlave = new VictorSPX(Constants.RIGHT_SLAVE_PORT);
  68. rightSlave.setNeutralMode(NeutralMode.Brake);
  69. //rightSlave.setInverted(true);
  70. rightSlave.follow(rightMaster);
  71.  
  72. gearSolenoid = new DoubleSolenoid(Constants.DRIVE_SOLENOID_FORWARD, Constants.DRIVE_SOLENOID_REVERSE);
  73.  
  74. navx = new AHRS(Port.kMXP);
  75. controller = new XboxController(Constants.PRIMARY_DRIVER_PORT);
  76. pid = new PIDController();
  77. //tXBuffer = new CircularBuffer(10);
  78.  
  79. tankEnabled = false;
  80. setpointReached = true;
  81. rotateSet = false;
  82. antiTiltEnabled = true;
  83. cargoSearch = false;
  84. leftTarget = 0;
  85. rightTarget = 0;
  86. lacc = 0;
  87. racc = 0;
  88. }
  89.  
  90. public static SWDrive getInstance() {
  91. return driveInstance;
  92. }
  93.  
  94. public void drive() {
  95. if(controller.getBackButtonReleased()) {
  96. setpointReached = true;
  97. super.flush();
  98. }
  99. if(controller.getStickButtonReleased(Hand.kLeft)) {
  100. PistonClimber.getInstance().toggleFrontPistons();
  101. }
  102. if(controller.getStickButtonReleased(Hand.kRight)) {
  103. PistonClimber.getInstance().toggleRearPiston();
  104. }
  105.  
  106.  
  107. if(!setpointReached || super.size() > 0) {
  108. if(super.size() > 0 && (target == null || setpointReached)) {
  109. target = super.pop();
  110. }
  111.  
  112. switch(target.getObjective()) {
  113. case DRIVELINEAR:
  114. autoDrive(target.getSetpoint());
  115. break;
  116. case DRIVEROTATE:
  117. autoRotate(target.getSetpoint());
  118. break;
  119. case CARGOSEARCH:
  120. rotateCargo(target.getSetpoint());
  121. default:
  122. break;
  123. }
  124. } else if(tankEnabled) {
  125. tankDrive();
  126. } else {
  127. arcadeDrive();
  128. }
  129.  
  130. if(controller.getPOV() >= 0 && !rotateSet) {
  131. //TODO: bug below, make it get angle right before it sets motors
  132. super.push(new RotateCmd(controller.getPOV() + navx.getYaw()));
  133. }
  134.  
  135. if(controller.getBumper(Hand.kLeft) && !cargoSearch) {
  136. super.push(new CargoSearchCmd(-1.0)); //TODO: Make enum for (counter)clockwise
  137. } else if(controller.getBumper(Hand.kRight) && !cargoSearch) {
  138. super.push(new CargoSearchCmd(1.0));
  139. }
  140.  
  141. if(controller.getXButtonReleased() && gearSolenoid.get() != Value.kForward) {
  142. gearSolenoid.set(Value.kForward);
  143. } else if(controller.getAButtonReleased() && gearSolenoid.get() != Value.kReverse) {
  144. gearSolenoid.set(Value.kReverse);
  145. }
  146.  
  147.  
  148. if(controller.getStartButtonReleased()) {
  149. tankEnabled = !tankEnabled;
  150. }
  151. if(controller.getYButtonReleased()) {
  152. antiTiltEnabled = !antiTiltEnabled;
  153. }
  154.  
  155. rotateSet = controller.getPOV() >= 0;
  156. cargoSearch = controller.getBumper(Hand.kLeft) || controller.getBumper(Hand.kRight);
  157. //tXBuffer.push(Limelight.getTx());
  158. }
  159.  
  160. public void autoDrive(double setpoint) {
  161. if(setpointReached) {
  162. leftTarget = leftMaster.getSelectedSensorPosition() + setpoint;
  163. rightTarget = rightMaster.getSelectedSensorPosition() + setpoint;
  164. setpointReached = false;
  165. }
  166.  
  167. leftMaster.set(ControlMode.Position, leftTarget);
  168. rightMaster.set(ControlMode.Position, rightTarget);
  169.  
  170. setpointReached = (Math.abs(leftMaster.getSelectedSensorPosition() - leftTarget) < Constants.LINEAR_EPSILON && Math.abs(rightMaster.getSelectedSensorPosition() - rightTarget) < Constants.LINEAR_EPSILON);
  171. }
  172.  
  173. public void rotateCargo(double direction) {
  174. if(setpointReached) {
  175. setpointReached = false;
  176. pid.start(Constants.ROTATE_GAINS);
  177. }
  178.  
  179. if(Limelight.isTarget()) {
  180. //use tXBuffer.getSmoothedValue() for smoothed??
  181. double normAngle = Utilities.normalizeAngle(Limelight.getTx());
  182. double output = pid.updateRotation(navx.getYaw(), normAngle);
  183.  
  184. driveMotors(output, -output);
  185. } else {
  186. double speed = direction * Constants.CARGO_SEARCH_ROTATE_SPEED;
  187. driveMotors(speed, -speed);
  188. }
  189.  
  190. setpointReached = Limelight.isTarget() && Math.abs(Limelight.getTx()) < Constants.CARGO_SEARCH_EPSILON;
  191. }
  192.  
  193. public void autoRotate(double theta) {
  194. if(setpointReached) {
  195. setpointReached = false;
  196. pid.start(Constants.ROTATE_GAINS);
  197. }
  198. double output = pid.updateRotation(navx.getYaw(), Utilities.normalizeAngle(theta));
  199.  
  200. driveMotors(output, -output);
  201.  
  202. setpointReached = Math.abs(pid.getError()) < Constants.ROTATE_EPSILON;
  203. }
  204.  
  205. public void zero() {
  206. navx.zeroYaw();
  207. leftMaster.setSelectedSensorPosition(0, 0, 0);
  208. rightMaster.setSelectedSensorPosition(0, 0, 0);
  209. }
  210.  
  211. public float getPitch() {
  212. return navx.getPitch();
  213. }
  214.  
  215. private void tankDrive() {
  216.  
  217. double leftY = Utilities.deadband(controller.getY(Hand.kLeft), 0.2);
  218. double rightY = Utilities.deadband(controller.getY(Hand.kRight), 0.2);
  219. double leftTrigger = Utilities.deadband(controller.getTriggerAxis(Hand.kLeft), 0.1);
  220. double rightTrigger = Utilities.deadband(controller.getTriggerAxis(Hand.kRight), 0.1);
  221.  
  222. double leftSpeed = leftY - leftTrigger + rightTrigger;
  223. double rightSpeed = rightY + leftTrigger - rightTrigger;
  224. double[] speeds = {-leftSpeed, -rightSpeed};
  225.  
  226. driveMotors(Utilities.normalize(speeds));
  227. }
  228.  
  229. private void arcadeDrive() {
  230. double leftY = -Utilities.deadband(controller.getY(Hand.kLeft), 0.2);
  231. double rightX = Utilities.deadband(controller.getX(Hand.kRight), 0.2);
  232.  
  233. double leftSpeed = leftY + Constants.TURNING_CONSTANT * rightX;
  234. double rightSpeed = leftY - Constants.TURNING_CONSTANT * rightX;
  235. double[] speeds = {leftSpeed, rightSpeed};
  236.  
  237. driveMotors(Utilities.normalize(speeds));
  238. }
  239.  
  240. private void driveMotors(double[] speeds) {
  241. driveMotors(speeds[0], speeds[1]);
  242. }
  243.  
  244. private void driveMotors(double leftSpeed, double rightSpeed) {
  245. float pitch = navx.getPitch();
  246.  
  247. if(Utilities.deadband(controller.getTriggerAxis(Hand.kLeft), 0.5) != 0) {
  248. leftSpeed = lacc * Constants.DECELERATION_CONSTANT;
  249. rightSpeed = racc * Constants.DECELERATION_CONSTANT;
  250. }
  251. racc = rightSpeed;
  252. lacc = leftSpeed;
  253.  
  254. if (antiTiltEnabled && Math.abs(pitch) > Constants.TILT_EPSILON && Math.abs(pitch) < 45) {
  255. double slope = (0.4 - 0.1) / (45 - Constants.TILT_EPSILON);
  256. double correctionOffset = slope * (pitch - Constants.TILT_EPSILON);
  257. double[] tmp = { leftSpeed + correctionOffset, leftSpeed + correctionOffset };
  258. Utilities.normalize(tmp);
  259. leftSpeed = tmp[0];
  260. rightSpeed = tmp[1];
  261. System.out.println(correctionOffset);
  262. }
  263.  
  264. leftMaster.set(ControlMode.PercentOutput, leftSpeed);
  265. rightMaster.set(ControlMode.PercentOutput, rightSpeed);
  266. }
  267.  
  268. private double getEncoderDiff() {
  269. return Math.abs(leftMaster.getSelectedSensorPosition(0) - rightMaster.getSelectedSensorPosition(0));
  270. }
  271.  
  272. public void rumbleController(double length) {
  273. (new JoystickRumble(controller, 1, length)).start();
  274. }
  275.  
  276. public boolean onDemandTest() { //TODO: Fix this
  277. double prevLeftEncoderCount = leftMaster.getSelectedSensorPosition(0);
  278. double prevRightEncoderCount = rightMaster.getSelectedSensorPosition(0);
  279. boolean leftHealthy = true, rightHealthy = true;
  280. driveMotors(0.1, 0.1);
  281. Timer.delay(3);
  282. if(Math.abs(prevLeftEncoderCount - leftMaster.getSelectedSensorPosition(0)) < 10) {
  283. System.out.println("LEFT MASTER ENCODER ERROR: \n NOT UPDATING FORWARD");
  284. System.out.println(" DIFF:" + Math.abs(prevLeftEncoderCount - leftMaster.getSelectedSensorPosition(0)));
  285. leftHealthy = false;
  286. }
  287. if(Math.abs(prevRightEncoderCount - rightMaster.getSelectedSensorPosition(0)) < 10) {
  288. System.out.println("RIGHT MASTER ENCODER ERROR: \n NOT UPDATING FORWARD");
  289. System.out.println(" DIFF:" + Math.abs(prevRightEncoderCount - rightMaster.getSelectedSensorPosition(0)));
  290. rightHealthy = false;
  291. }
  292. if(getEncoderDiff() > 0.1 * Math.max(Math.abs(leftMaster.getSelectedSensorPosition(0)), Math.abs(rightMaster.getSelectedSensorPosition(0)))) {
  293. System.out.println("DRIVEBASE SIDE DIFFERENCE: \n DIFFERENCE BETWEEN SIDES FORWARD");
  294. System.out.println(" DIFF:" + getEncoderDiff());
  295. rightHealthy = false;
  296. leftHealthy = false;
  297. }
  298. driveMotors(-0.1, -0.1);
  299. Timer.delay(3);
  300. if(Math.abs(prevLeftEncoderCount - leftMaster.getSelectedSensorPosition(0)) < 10) {
  301. System.out.println("LEFT MASTER ENCODER ERROR: \n NOT UPDATING REVERSE");
  302. System.out.println(" DIFF:" + getEncoderDiff());
  303. leftHealthy = false;
  304. }
  305. if(Math.abs(prevRightEncoderCount - rightMaster.getSelectedSensorPosition(0)) < 10) {
  306. System.out.println("RIGHT MASTER ENCODER ERROR: \n NOT UPDATING REVERSE");
  307. System.out.println(" DIFF:" + getEncoderDiff());
  308. rightHealthy = false;
  309. }
  310. if(getEncoderDiff() > 0.1 * Math.max(Math.abs(leftMaster.getSelectedSensorPosition(0)), Math.abs(rightMaster.getSelectedSensorPosition(0)))) {
  311. System.out.println("DRIVEBASE SIDE DIFFERENCE: \n DIFFERENCE BETWEEN SIDES REVERSE");
  312. System.out.println(" DIFF:" + getEncoderDiff());
  313. rightHealthy = false;
  314. leftHealthy = false;
  315. }
  316. driveMotors(0, 0);
  317. Timer.delay(3);
  318. prevLeftEncoderCount = leftMaster.getSelectedSensorPosition(0);
  319. prevRightEncoderCount = rightMaster.getSelectedSensorPosition(0);
  320. if(Math.abs(prevLeftEncoderCount - leftMaster.getSelectedSensorPosition(0)) != 0) {
  321. System.out.println("LEFT MASTER ENCODER ERROR: \n UPDATING WHEN OFF");
  322. System.out.println(" DIFF:" + Math.abs(prevLeftEncoderCount - leftMaster.getSelectedSensorPosition(0)));
  323. leftHealthy = false;
  324. }
  325. if(Math.abs(prevRightEncoderCount - rightMaster.getSelectedSensorPosition(0)) != 0) {
  326. System.out.println("Right MASTER ENCODER ERROR: \n UPDATING WHEN OFF");
  327. System.out.println(" DIFF:" + Math.abs(prevRightEncoderCount - rightMaster.getSelectedSensorPosition(0)));
  328. rightHealthy = false;
  329. }
  330.  
  331. return leftHealthy && rightHealthy;
  332. }
  333. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!