Help for my shaking quadruped robot

Other Hardware Robotics
10

I spotted this:

else if(distance <= CRITdistance) {
        CRITvar = 1;                    // you assign a value
        switch(CRITvar) {            // then immediately test for its value
        case 1 :                            // of course this case will always be selected
            resetPREVangle();
            prevmillis = millis();
            while(millis() - prevmillis < 5000) {
                walkBWD();
                CRITvar = 2;
            }                          // missing "break", so case 2: will also be executed

        case 2 :
            resetPREVangle();
            ultrasoundSWITCH = 1;
            stallVAR = 0;
            turnRightVAR = 0;
            turnLeftVAR = 0;

            rotate();
            break;
        }
    }

CRITVar is not used elsewhere, so the code can be simplified as follows:

    else if(distance <= CRITdistance) {
            resetPREVangle();
            prevmillis = millis();
            while(millis() - prevmillis < 5000) {
                walkBWD();
            }

            resetPREVangle();
            ultrasoundSWITCH = 1;
            stallVAR = 0;
            turnRightVAR = 0;
            turnLeftVAR = 0;

            rotate();
    }

my 1 cent :slight_smile: