Когда я использую + =, чтобы добавить число в массив, оно всегда заканчивается 0, независимо от того, какое число

Question

Я пытаюсь написать код для своего робота, который отслеживает, куда он ехал, чтобы он мог знать, где он находится в декартовой сетке (я основываюсь на этом документе: http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf).

В начале absolute_position[0] (x-coordinate) и absolute_position[1] (y-coordinate) оба равны set to 2, что означает робота starts at (2,2) в сетке. В первом цикле , до того, как робот переместился (имеется в виду влево, вправо и назад variables are all set to 0), absolute_position[0] and absolute_position[1] are both set to 0.

Насколько я могу судить, varCos и varSin должны быть равны 0, поэтому

absolute_position[0] += varCos;
absolute_position[1] += varSin;

следует оценить до

2 += 0;
2 += 0;

но, как я уже сказал, они оба заканчиваются на 0. если я попытаюсь заменить varCos и varSin на 0, или если я установлю для них обоих значение 0 (а не offset_global [0] * cos (offset_global [1])), абсолютное_положение окажется [2,2], как и следовало ожидать.

Полный код:

double absolute_position[2] = {2,2};
double theta0;
double left = 0;
double right = 0;
double back = 0;
double prevLeft = 0;
double prevRight = 0;
double prevBack = 0;
double deltaLr = 0;
double deltaRr = 0;
double deltaBr = 0;
double deltaL = 0;
double deltaR = 0;
double deltaB = 0;
double thetar = 0;
double theta1 = 0;
double deltaTheta = 0;
double thetaM = 0;
double offset_local[2];
double offset_global[2];
double varCos = 0;
double varSin = 0;

///////////////////////////////////////////////////

void positionTracking(){
    float sL = 10.5;
    float sR = 10.5;
    float sB = 6.5;
    while(true){
        //stores current encoder values
        left = -leftEncoder;
        right = rightEncoder;                                                   
        back = backEncoder;

        //finds the distance traveled for each wheel in inches
        deltaL = (left - prevLeft) * 3.25 * M_PI / 360;
        deltaR = (right - prevRight) * 3.25 * M_PI / 360;
        deltaB = (back - prevBack) * 3.25 * M_PI / 360;

        //updates the last values of the encoders to be used next cycle
        prevLeft = left;
        prevRight = right;
        prevBack = back;

        //calculates total accumulated encoder values
        deltaLr += deltaL;
        deltaRr += deltaR;
        deltaBr += deltaB;

        //calculates new absolute orientation
        theta1 = thetar + (deltaLr - deltaRr) / (sL + sR);
        if(theta1 < 0){
            theta1 += 2 * M_PI;
        }
        else if(theta1 >= 2 * M_PI){
            theta1 -= 2 * M_PI;
        }

        //find the change in orientation
        deltaTheta = theta1 - theta0;

        //find local offset vector
        if(deltaTheta == 0){
            offset_local[0] = deltaB;
            offset_local[1] = deltaR;
        }
        else{
            offset_local[0] = 2 * sin(deltaTheta / 2) * (deltaB / deltaTheta + sB);
            offset_local[1] = 2 * sin(deltaTheta / 2) * (deltaR / deltaTheta + sR);
        }

        //calculate the average orientation
        thetaM = theta0 + deltaTheta / 2;

        //converts cartesian to polar and changes the angle
        offset_global[0] = sqrt(pow(offset_local[0], 2) + pow(offset_local[1], 2));
        offset_global[1] = atan(offset_local[1] / offset_local[0]) - thetaM;

        //converts polar offset back to cartesian and adds it to the absolute_position
        varCos = offset_global[0] * cos(offset_global[1]);
        varSin = offset_global[0] * sin(offset_global[1]);

        absolute_position[0] += varCos;
        absolute_position[1] += varSin;

        //updates the old orientation to be used next cycle
        theta0 = theta1;
    }
}

Спасибо за любую помощь!

Answer 1

Я понял это: на экране робота было указано, что varCos и varSin равны 0, но когда я запустил программу на своем компьютере (а не на роботе), он сказал, что это NaN. Я изменил это на

varCos = offset_global[0] == 0 ? 0 : offset_global[0] * cos(offset_global[1]);
varSin = offset_global[0] == 0 ? 0 : offset_global[0] * sin(offset_global[1]);

, чтобы убедиться, что они числа.