基于PID的java机器人转向

Question

我目前有一个PID算法来控制我的机器人在自主状态下转弯。我的机器人有编码器，每个马达上有四个，还有一个BNO055IMU。此外，每个电机都是Andymark的一个永不休息的40电机，不幸的是，我被编码器卡住了，它可以产生3个脉冲。我想通过使用不同的算法或改进我当前的算法来提高转弯的准确性

我当前的转弯代码：

 public void turn(int angle, Direction DIRECTION, double timeOut, int sleepTime, double kp, double ki, double kd) {
    double targetAngle = imu.adjustAngle(imu.getHeading() + (DIRECTION.value * angle));
    double acceptableError = 0.5;
    double currentError = 1;
    double prevError = 0;
    double integral = 0;
    double newPower;
    double previousTime = 0;
    timeoutClock.reset();
    while (opModeIsActive() && (imu.adjustAngle(Math.abs(currentError)) > acceptableError)
            && !timeoutClock.elapsedTime(timeOut, MasqClock.Resolution.SECONDS)) {
        double tChange = System.nanoTime() - previousTime;
        previousTime = System.nanoTime();
        tChange = tChange / 1e9;
        double imuVAL = imu.getHeading();
        currentError = imu.adjustAngle(targetAngle - imuVAL);
        integral += currentError * ID;
        double errorkp = currentError * kp;
        double integralki = integral * ki * tChange;
        double dervitive = (currentError - prevError) / tChange;
        double dervitivekd = dervitive * kd;
        newPower = (errorkp + integralki + dervitivekd);
        newPower *= color;
        if (Math.abs(newPower) > 1.0) {newPower /= newPower;}
        driveTrain.setPower(newPower, -newPower);
        prevError = currentError;
        DashBoard.getDash().create("TargetAngle", targetAngle);
        DashBoard.getDash().create("Heading", imuVAL);
        DashBoard.getDash().create("AngleLeftToCover", currentError);
        DashBoard.getDash().update();
    }
    driveTrain.setPower(0,0);
    sleep(sleepTime);
}

注: 当动力传动系。设定功率（x，y）；称为左参数是设置到左侧的功率，右参数设置到右侧

方向是一个枚举，用于存储wither-1，或用于在左转和右转之间切换的1

仪表板。格达什。create只是记录正在发生的事情

伊莫大学。adjustAngle执行以下操作：

    public double adjustAngle(double angle) {
    while (angle > 180)  angle -= 360;
    while (angle <= -180) angle += 360;
    return angle;
}

伊莫大学。getHeading（）是不言自明的，它获取机器人的偏航

我的pid常量的当前值。（它们工作得很好。）

KP_TURN = 0.005, KI_TURN = 0.0002, KD_TURN = 0, ID = 1;