/** * @param wheelDiameter * @param gearRatio * @param threshold Amount of allowable error in number of encoder ticks to destination. * @param slowDownStart Number of wheel revolutions before slowing down the first time. * @param fineTuneStart Number of wheel revolutions before slowing down the second time. * @param typePID Determines conversion factor (degrees to ticks or distance to ticks based off * type) * @param motors */ public EncoderPIDController( double wheelDiameter, int gearRatio, double threshold, double slowDownStart, double fineTuneStart, TypePID typePID, DcMotor... motors) { super(threshold, motors); boolean isSymmetrical = (motors.length % 2 == 0); int sides = 1; if (isSymmetrical) { sides = 2; } super.setSides(sides); this.wheelDiameter = wheelDiameter; this.gearRatio = gearRatio; this.slowDownStart = slowDownStart; this.fineTuneStart = fineTuneStart; this.typePID = typePID; switch (typePID) { case DRIVE: super.setConversionFactor(TICKS_PER_REVOLUTION / (wheelDiameter * Math.PI * gearRatio)); Log.d("SetTargets", "" + conversionFactor); break; case LIFT: break; default: break; } targets = new double[sides]; }
public void goToPosition(String sPosition) { m_sPosition = sPosition; pidLeft.setSetpoint(m_preferences.getFloat(sPosition, 0)); pidRight.setSetpoint(m_preferences.getFloat(sPosition + "R", 0)); System.out.println(sPosition + " Setpoint = " + m_preferences.getFloat(sPosition, 0)); }
public void navigateRobot() { System.out.println("Path Index: " + pathIndex); System.out.println("goalPath Size: " + goalPath.size()); System.out.println("locX, locY, locTheta" + locX + ", " + locY + ", " + locTheta); System.out.println("goalX, goalY, goalTheta: " + goalX + ", " + goalY + ", " + goalTheta); if (goalPath.size() > 0 && !done) { if (pathIndex < goalPath.size() - 1 && Math.abs(goalX - locX) < 0.2 && Math.abs(goalY - locY) < 0.2) { pathIndex++; } if (pathIndex == goalPath.size() - 1 && Math.abs(goalX - locX) < 0.05 && Math.abs(goalY - locY) < 0.05) { pathIndex++; } if (pathIndex >= goalPath.size()) { done = true; setVelocities(0.0, 0.0); } goalX = goalPath.get(pathIndex).x; goalY = goalPath.get(pathIndex).y; goalTheta = Math.atan2(goalY - locY, goalX - locX); if (goalTheta < 0) { goalTheta += 2 * Math.PI; } double error = goalTheta - locTheta; if (error > Math.PI) { error -= 2 * Math.PI; } else if (error < -Math.PI) { error += 2 * Math.PI; } if (Math.abs(error) < Math.PI / 8.0) { setVelocities(0.3, anglePID.update(error)); } else { setVelocities(0, anglePID.update(error)); } // setVelocities(0.2, anglePID.update(error)); } }
// Initialize your subsystem here public LoaderClimberOld2PID() { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID // super("LoaderClimberOld2PID", 1.5, 0.1, 0.0); // setAbsoluteTolerance(0.02); // getPIDController().setContinuous(false); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID m_preferences = Preferences.getInstance(); if (leftJag != null) { // for practice robot // Fix for motors going in opposite directios PIDOutput rightOutput = new PIDOutput() { public void pidWrite(double output) { rightJag.pidWrite(-1 * output); } }; pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/ LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft); LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight); pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f)); pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f)); pidLeft.setInputRange(0, 4); pidRight.setInputRange(0, 4); pidLeft.setPercentTolerance(PID_TOL); pidRight.setPercentTolerance(PID_TOL); pidLeft.enable(); // - Enables the PID controller. pidRight.enable(); // - Enables the PID controller. // disable(); } }
public boolean anglegood() { return pidLeft.onTarget() && pidRight.onTarget(); }