Question

我目前有一个PID算法来控制我的机器人在自治状态下转弯。我的机器人在每个电机上都有编码器，有4个，还有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);
}

注意：当driveTrain.setPower（x，y）;被称为左参数是设置在左侧的功率，右参数设置在右侧。

Direction是一个枚举，它存储-1或1以在左右转弯之间切换。

Dashboard.getDash.create仅用于记录正在发生的事情。

imu.adjustAngle执行以下操作：

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

imu.getHeading（）是自我解释的，它可以获得机器人的偏航。

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

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

机器人使用PID转动

0 个答案: