The EQ My Block provides an equation that controls the power of the motors. This equation determines if the robot needs to accelerate to stay upright. We are solving the inverted pendulum problem with this calculation. By the end of the EQ My Block, a new value is generated for the pwr variable block (the power variable).

A series of three Advanced Math blocks are used to calculate the value of the power variable, which in turn leads to the control of the motors. Although the Cdrv variable block affects how we control whether the robot drives forward or backwards, it is the value of the pwr variable block that sets the level of motor speed to keep the robot upright. Essentially, we are trying to determine whether the robot needs to accelerate to stay upright. If the Advanced Math block could handle more inputs, then only one Advanced Math block would be needed. The value of the power variable is based on input from several numeric variable blocks, including the Cdrv, time integral (tInt), Gyro angle (gAng), Gyro speed (gSpd), motor position (mPos), and motor speed (mSpd). Each of these variables provides sensor feedback to the value of the motor power variable. Embedded into the Advanced Math blocks are several gain constants and coefficients, which tune and optimize the robot. If the program were well written, these gain constants would be defined as distinct variables instead of being hidden inside of the math functions.

In the first part of the EQ My Block, the motor position variable is updated by integrating the control driving rate variable over a certain period of time. A simple physics model for this integral calculation would be that distance is speed multiplied by time. The Math block multiplies the tInt variable block by the Cdrv variable block (speed). This factor is subtracted from the previous value of the mPos variable block to return a new value for the motor position.

Next, the weighted values of each of the variable blocks (gAng, gSpd, mPos, mSpd, and Cdrv) are added together to obtain a value for the pwr variable block. They are weighted by multiplying the values by their respective gain constants inside of the Advanced Math blocks. Of these, you might notice that the Gyro angle variable, gAng, has a gain value much larger than the other feedback terms. That is largely because of its primary importance. We want the robot to stay upright and have a very small (if not zero) value for the Gyro angle. The motor speed and position variables are important because if we want to accelerate the robot, it is helpful to know the current motor speed and position.

The final steps in this My Block are to put limits on the power variable. The maximum value possible for the motor power is 100. The first compare block determines whether the value of the pwr variable block is greater than 100. If it is greater, then the logic switch block overwrites the pwr variable block with a value of 100. Then, in the opposite direction, if the power variable is less than -100, the pwr variable block is overwritten with the value -100. The false case of these logic switch blocks are empty, which is why I have presented them in a tabbed view in the preceding screenshot.