using System;

public class Program

{

    public static void Main()

    {

        Console.WriteLine("Calibration 1:");

        // Pt 1

        CalibrationData.CustomerCalibrationSetPoint(1, 10, 10.5);       

        // Pt 2

        CalibrationData.CustomerCalibrationSetPoint(2, 25, 24.6);

        Console.WriteLine(Environment.NewLine + "Calibration 2:");

        // Pt 1

        CalibrationData.CustomerCalibrationSetPoint(1, 10, 9.8);        

        // Pt 2

        CalibrationData.CustomerCalibrationSetPoint(2, 25, 25.1);

    }

}

​

public static class CalibrationData

{

    private static double reference1 = 0;

    private static double measured1 = 0;

    private static short registerK = 8192;

    private static short registerD = 0;

    public static void CustomerCalibrationSetPoint(int currentPoint, double reference, double measured)

    {       

        if (currentPoint == 1)

        {

            // Only store first value and reference

            measured1 = measured;

            reference1 = reference;

            Console.WriteLine($"Reference1 = {reference}, Measured1 = {measured}");

        }

        else

        {

            Console.WriteLine($"Reference2 = {reference}, Measured2 = {measured}");

            // Get old values from current register values

            double kOld = registerK / Math.Pow(2, 13);

            double dOld = registerD / 100.0;

            Console.WriteLine($"Old values: K = {kOld}, D = {dOld}");

            // Calculate new values

            double reference2 = reference;

            double measured2 = measured;

            double kNew = (reference2 - reference1) / (measured2 - measured1) * kOld;

            double dNew = reference1 - ((measured1 - dOld) * kNew / kOld);

            Console.WriteLine($"kNew = {kNew}, dNew = {dNew}");

​

            SetRegister(kNew, dNew);

        }

    }

    private static void SetRegister(double k, double d)

    {

        registerK = (short)(k * Math.Pow(2, 13));

        registerD = (short)(d * 100.0);

        Console.WriteLine($"Writing register values: K = {registerK}, D = {registerD}");

    }

}