These P being so much bigger than I are going to be very volatile (lots of big ups and downs). I think these should all be primarily I with much smaller P coefficients.
Don't change state from outside signal. We should just set a requestBPMeasurement flag to TRUE in this function (if current state is IDLE) and then when the state machine runs next, the IDLE state should look for this flag to be TRUE and transition to MEASURE state (by doing all of these things you have here in this function).
