The answer (to me) is simple - Design the code to run in an iterative loop, and process all subsystems in each loop iteration.
I use a hybrid method where all mechanisms are handled asynchronously and navigation is handled with blocking commands. I use RobotC, so my process functions are all poked from another task, but there is no reason (other than simplicity) why I couldn’t call them from my nav functions.
Do this:
-Write a few functions to process your mechanisms (P or PID control for arms, state-machines to run rollers) in such a way that they rely on simple input via globally-defined variables from other systems - Roller has a single power variable, arm has single setpoint variable. These “poke” functions do not contain while loops, but are designed to be called in while loops.
-Navigation commands (drive_straight, turn) have the only while loops in autonomous, and they handle the drivetrain directly. They also call the poke functions of the other subsystems.
In autonomous, you can do something like:
roller_state = 0;//Roller Off
arm_setpoint = 2000;//Let's say this is an analog value representing the height we want to go to
nav_drive_straight(42);//Let's say this drives inches
roller_state = 127;//Spit
nav_drive_straight would be something like this:
void nav_drive_straight(float dist){
//Find counts
dist *= 84;//This is the encoder counts per inch, to turn inches to counts
signed char speed = sign(dist) * 127;//+ or - 127 depending on sign of distnace
while(1)
{
SetMotor(1,speed);SetMotor(2,speed);///Set all of the drive motors
if(Abs(GetQuadEncoder(1,2)) >= Abs(dist)) break;
arm_process();
roller_process();
Wait(20);//Try to get a consistent loop time in all loops so PID can function properly
}
SetMotor(1,0);SetMotor(2,0);//Set all motors to 0
}
And arm_process() could be something like a Proportional controller:
void arm_process() {
//Local variables
int arm_sensor,arm_error,arm_output;
arm_sensor = GetAnalogValueHR(1);//Read arm sensor - HR reads in 12 bit resolution instead of 10
arm_error = arm_setpoint - arm_sensor;//Remember that setpoint is globally defined
arm_output = arm_error * ARM_KP;//ARM_KP is a #defined gain constant
SetMotor(5,arm_output);//Set motor
}
I have even more modular code where I define a state-machine which sets the setpoint, and a typedef enum which defines the desired state, and a PID controller which holds the state, and all I have to do at any time is say
arm_write_state(stateCOLLECT);//Floor collection position
arm_map_dig_to_state(GetJoystickDigital(2,8,1),stateCOLLECT);//This would set stateCOLLECT only when that button is pressed
and the rest of it is handled for me by the processing functions automagically. The arm_map_dig_to_state command is really awesome, as I can define my whole HMI in like 5 lines, just by calling that function 5 times in a row with each of the button values and the state that button maps to.