Today I wanted to document how the auto-stop code works, this is now one of my favorite features of SAMI because it allows to do any other thing in the main controller while SAMI takes care of speed and traveled distance and then stop automatically using precision PID control. With some extra sensors like an IMU and a little bit of coordination in the main control to all SAMI's it can really do a big difference when controlling robot movements. 

So here is the code that powers this feature:

if (I2C_buffer.data.DISTANCE != 0 && loadDistance == 0) {
                        loadDistance = 1;
                        auxDistance = I2C_buffer.data.DISTANCE;
                        I2C_buffer.data.DISTANCE = 0;
                        stable = 0;
                    } else {
                        calculate_pidM(calculate_pidA(auxDistance));
                        if (auxDistance >= I2C_buffer.data.DISTANCE - ATS_tolerance && auxDistance <= I2C_buffer.data.DISTANCE + ATS_tolerance) {
                            stable++;
                            if (stable > stable_time) //travel distance stable at desired value for at least some time
                            {
                                loadDistance = 0;
                                M_control(0);
                                I2C_buffer.data.START_STOP = 0;
                            }
                        } else {
                            stable = 0;
                        }
                    }

As you can see when you first set the desired distance it loads into a temporal variable and then clears the register and sets a flag to know that it has been set-up. After that the algorithm starts running, we use the previously discussed PID control equations, but this time using the desired distance as the setpoint, the encoder traveled distance as an input and the output is going to be the RPM's to run the motor. They can't be higher that the RPM's limit set by the user. 

Now this is the important part, we have a flag to stop the motor when we reach the desired position, otherwise it will run forever making the motor overheat and also shaky due to the algorithm trying to hold the position. So to prevent that I have make a clever condition that basically waits some time before disabling the controller only if it is stable in the desired position. Obviously the desire position has a range up and down, because it will be almost impossible for a common DC motor to make too precise movements.

After the algorithm stops the motor when done, also clears the running condition flag in the driver. So now the host controller will now that SAMI has done and is ready to receive a new order. Pretty useful for sequencing moves in a robot without the "delay thing" hassle.