The
GM My Block gets motor shaft encoder sensor feedback and calculates values to store to the motor position and motor speed variables. The GM My Block does not measure the motor speed directly in the same way the GG My Block does. Instead, it calculates the derivative of the motor positions, or how much the values of the motor positions have changed over the time of the balance loop iterations. The motor position is almost a direct read from the shaft encoders with a slight tweak to account for historical variations between the two motors due to the turning of the robot. Having the current motor positions and speeds will become important when trying to calculate the needed accelerations to the motors to maintain an upright position.
The motor speed calculation begins by recalling the sum of the two motor shaft encoder values from the previous iteration using the mSUM variable block. Although the mSUM variable block is written with this sum, if you trace the data wires,...
