Exoskeleton Motor Control Code
Here's what I came up with:


You have a certain percent of the gain cycle equal one "step" (step may be a bad term in this situation, but I'm using it because of stepper motors).
You have a blocking function that will move the motors and control all the P(ID) logic

Before this function is called, the main code will use a variable to store the desired time to do the next "step."
It will have a loop with "substeps." So it would look kinda like this:





void function nextStep(int var1, int var2) {
    //Read sensors and put into variables
    until(condition == true) { //Condition to see if the motors are at the right degree
        //Adjust motors not in the right place
        delayAndCheckBT(10); //Look at the bottom for this function
    }
}


You could also have it accept a few percent of variance as tolerance so it's not shakey, but that's more of the PID control than anything. You'll have to make sure that many 'steps' doesn't make the PID controller unstable.


When that code is done, the main function will check to see if the desired time for the next step has passed (or if it's now). If it's passed/now, then it will execute. Otherwise, it will not execute the next step and continue executing the main loop.


Great? No. The problem is you need to check for bluetooth! To do this, I've made two functions:

void checkBT() {
    //Checks BT. Make sure this is non blocking: if you need to, set a few global variables
    //Note this code will get called a LOT, so don't implement any delays or loops: inside here: use millis().
}
void delayAndCheckBT(int timeToWait) {
    unsigned long finishTime = millis() + timeToWait;
    while (millis()&lt;finishTime) {
        checkBT();
    }
}