@@ -0,0 +1,2003 @@
+package org.camsrobotics.frc2016.subsystems.controllers;%0D%0A%0D%0Aimport org.camsrobotics.frc2016.Constants;%0D%0Aimport org.camsrobotics.frc2016.subsystems.Drive.DriveController;%0D%0Aimport org.camsrobotics.frc2016.subsystems.Drive.DriveSensorSignal;%0D%0Aimport org.camsrobotics.frc2016.subsystems.Drive.DriveSignal;%0D%0Aimport org.camsrobotics.lib.NerdyMath;%0D%0Aimport org.camsrobotics.lib.NerdyPID;%0D%0A%0D%0A/**%0D%0A * PID Drive Straight to distance Controller%0D%0A * %0D%0A * @author Wesley%0D%0A *%0D%0A */%0D%0Apublic class DriveStraightDistanceController implements DriveController %7B%0D%0A%09private final double m_kRotP = Constants.kDriveRotationP;%0D%0A%09private final double m_kRotI = Constants.kDriveRotationI;%0D%0A%09private final double m_kRotD = Constants.kDriveRotationD;%0D%0A%09%0D%0A%09private final double m_kDistP = Constants.kDriveTranslationP;%0D%0A%09private final double m_kDistI = Constants.kDriveTranslationI;%0D%0A%09private final double m_kDistD = Constants.kDriveTranslationD;%0D%0A%09%0D%0A%09private double m_tolerance;%0D%0A%09private double m_rotPower = 0;%0D%0A%09private double m_straightPower = 0;%0D%0A%09%0D%0A%09private NerdyPID m_pidRotController;%0D%0A%09private NerdyPID m_pidDistController;%0D%0A%09%0D%0A%09public DriveStraightDistanceController(double tolerance, double distance)%09%7B%0D%0A%09%09m_tolerance = tolerance;%0D%0A%09%09%0D%0A%09%09m_pidDistController = new NerdyPID(m_kDistP, m_kDistI, m_kDistD);%0D%0A%09%09m_pidDistController.setDesired(distance);%0D%0A%09%09%0D%0A%09%09m_pidRotController = new NerdyPID(m_kRotP, m_kRotI, m_kRotD);%0D%0A%09%09m_pidRotController.setDesired(0);%0D%0A%09%7D%0D%0A%09%0D%0A%09@Override%0D%0A%09public DriveSignal get(DriveSensorSignal sig) %7B%0D%0A%09%09m_rotPower = m_pidRotController.calculate((sig.yaw + 360) %25 360);%0D%0A%09%09m_straightPower = m_pidDistController.calculate((sig.leftEncoderDistance + sig.rightEncoderDistance)/2);%0D%0A%09%09double%5B%5D pow = %7Bm_rotPower + m_straightPower, -m_rotPower + m_straightPower%7D;%0D%0A%09%09pow = NerdyMath.normalize(pow, false);%0D%0A%09%09return new DriveSignal(pow%5B0%5D, pow%5B1%5D);%0D%0A%09%7D%0D%0A%0D%0A%09@Override%0D%0A%09public boolean isOnTarget() %7B%0D%0A%09%09return m_pidDistController.onTarget(m_tolerance) && m_pidRotController.onTarget(m_tolerance);%0D%0A%09%7D%0D%0A%7D%0D%0A

add drive strait to distance