0

我目前有一个 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);
}

注意：当 driveTrain.setPower(x,y); 被称为左侧参数是设置到左侧的功率，右侧参数设置右侧。

方向是一个枚举，存储凋零 -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;

4

0 回答 0