#define RAD_WHEEL 0.028 // радиус колес
#define DEG2RAD PI / 180
#define LEFT OUT_C
#define RIGHT OUT_B
#define GYRO S1
#define LIMIT 20 // предельное растояние до препятствия
#define K_I 50 // коэффициент интегральной составляющей линейной скорости
#define K_P 0.1 // коэффициент пропорциональной составляющей курсового угла
#define ERROR 0.03 // точность достижения целевой точки

task main() {
    int     rotA, rotB, // показания энкодеров
            pwmLeft, pwmRight, // ШИМ для управления
            distS3, distS2, distMax, kDist, // ошибки расстояния до препятствия
            currentTime, previousTime, dt, // время
            gyroSpeed, gyroOffset, // угловая скорость с гироскопа и ее калибровка
            fileSize = 30640;

    float   course, courseAngle, bearing, // курс (psi), курсовой угол (alpha), пеленг (theta)
            xCoord, yCoord, // координаты точки между колесами
            delthaX, delthaY, // отклонения
            length, intLength, // длина оставшегося пути, интеграл длины
            path, prevPath, delthaPath, // путь пройденный точкой между колесами
            xRef = -1, yRef = 0, // координаты точки назначения
            baseSpeed = 0, control = 0, // линейная и угловая скорость
            k = 20;

    string  s; // строка для формирования вывода

    byte    handle;

    // инициализация датчиков
    SetSensorLowspeed(S2);
    SetSensorHTGyro(GYRO);

    // создание файла для записи данных, задержка
    DeleteFile("data.txt");
    CreateFile("data.txt", fileSize, handle);
    Wait(50);

    // калибровка гироскопа, угловая скорость (град/сек)
    gyroOffset = SensorHTGyro(GYRO);

    course = 0;
    previousTime = CurrentTick();

    while (true) {
        // измерение ошибки расстояния до препятствия
        distS3 = LIMIT - SensorUS(S3);
        distS2 = LIMIT - SensorUS(S2);

        // определения положения препятствия (с какой стороны робота, далеко ли от него)
        if (distS3 < 0 ) distS3 = 0;
        if (distS2 < 0 ) distS2 = 0;
        if (distS2 > distS3) distMax = distS2;
        else distMax = distS3;
        kDist = sign(distS2 - distS3);

        currentTime = CurrentTick();
        dt = currentTime - previousTime;
        previousTime = currentTime;

        // получение угловой скорости (град/сек)
        gyroSpeed = SensorHTGyro(GYRO) - gyroOffset;
        // расчет курса
        course = course + gyroSpeed * PI * dt / 1000.0 / 180.0;

        // снятие показаний энкодеров с двигателей
        rotA = MotorRotationCount(LEFT);
        rotB = MotorRotationCount(RIGHT);

        // расчет пути пройденного точкой между колесами
        path = (rotA + rotB) * DEG2RAD * RAD_WHEEL / 2;

        // путь пройденный за один цикл программы
        delthaPath = path - prevPath;

        // сохранение предыдущего значения длины пути
        prevPath = path;

        // вычисление координаты X
        xCoord = xCoord + delthaPath * cos(course);

        // вычисление координаты Y
        yCoord = yCoord + delthaPath * sin(course);

        // вычисление отклонения по X
        delthaX = xRef - xCoord;

        // вычисление отклонения по Y
        delthaY = yRef - yCoord;

        // расчет пеленга
        bearing = atan2(delthaY, delthaX);

        // расчет курсового угла
        courseAngle = bearing - course - K_P * kDist * distMax;

        // разворот по кратчайшему углу
        if (abs(courseAngle) > PI) courseAngle = courseAngle - sign(courseAngle) * 2 * PI;

        // расчет расстояния до точки
        length = sqrt (delthaY * delthaY + delthaX * delthaX);
        intLength = intLength + length * dt / 1000;
        if (intLength > 10) intLength = 10;

        // расчет линейной скорости
        baseSpeed = 100 * tanh (length) * cos (courseAngle) + K_I * intLength;
        if (abs(baseSpeed) > 40) baseSpeed = sign (baseSpeed) * 40;

        // расчет угловой скорости
        control = k * courseAngle + sin(courseAngle) * baseSpeed / length;

        // насыщение управляющего воздействия
        if (abs(control) > 30) control = sign(control)*30;

        // преобразование управления из float в integer, ШИМ в процентах
        pwmLeft = baseSpeed + control;
        // преобразование управления из float в integer, ШИМ в процентах
        pwmRight = baseSpeed - control;

        if (abs(pwmLeft) > 100) pwmLeft = sign (pwmLeft) * 100;
        if (abs(pwmRight) > 100) pwmLeft = sign (pwmRight) * 100;

        // подача управления
        OnFwd(LEFT, pwmLeft);
        OnFwd(RIGHT, pwmRight);
        ClearScreen();

        // вывод значений на экран NXT
        NumOut(0, 8, distS3);
        NumOut(0, 0, distS2);

        // создание строки с данными
        s = NumToStr(xCoord) + " " + NumToStr(yCoord) + " " + NumToStr(bearing) + " " + NumToStr(course);

        // запись в файл
        WriteLnString(handle, s, fileSize);

        // выход при достижении области точки
        if(abs(length) < ERROR) {
            Off(OUT_BC);
            break;
        }
        Wait(5);
    }
}