当我使用+ =向数组添加数字时，无论数字是多少，它总是最终为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（而不是offset_global [0] * cos（offset_global [1]）），它们最终都为0.，absolute_position结束像我期望的那样[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]);

确保他们是数字。