//This duplicates functionalist in leg_ctrl
static void hallUpdateBEMF() {
//Back EMF measurements are made automatically by coordination of the ADC, PWM, and DMA.
//Copy to local variables. Not strictly neccesary, just for clarity.
//This **REQUIRES** that the divider on the battery & BEMF circuits have the same ratio.
hallbemf[0] = adcGetVBatt() - adcGetBEMFL();
hallbemf[1] = adcGetVBatt() - adcGetBEMFR();
//NOTE: at this point, we should have a proper correspondance between
// the order of all the structured variable; bemf[i] associated with
// pidObjs[i], bemfLast[i], etc.
// Any "jumbling" of the inputs can be done in the above assignments.
//Negative ADC measures mean nothing and should never happen anyway
if (hallbemf[0] < 0) {
hallbemf[0] = 0;
}
if (hallbemf[1] < 0) {
hallbemf[1] = 0;
}
//Apply median filter
int i;
for (i = 0; i < NUM_HALL_PIDS; i++) {
hallbemfHist[i][2] = hallbemfHist[i][1]; //rotate first
hallbemfHist[i][1] = hallbemfHist[i][0];
hallbemfHist[i][0] = hallbemf[i]; //include newest value
hallbemf[i] = medianFilter3(hallbemfHist[i]); //Apply median filter
}
// IIR filter on BEMF: y[n] = 0.2 * y[n-1] + 0.8 * x[n]
hallbemf[0] = (2 * (long) hallbemfLast[0] / 10) + 8 * (long) hallbemf[0] / 10;
hallbemf[1] = (2 * (long) hallbemfLast[1] / 10) + 8 * (long) hallbemf[1] / 10;
hallbemfLast[0] = hallbemf[0]; //bemfLast will not be used after here, OK to set
hallbemfLast[1] = hallbemf[1];
}
void updateBEMF(){
//Back EMF measurements are made automatically by coordination of the ADC, PWM, and DMA.
//Copy to local variables. Not strictly neccesary, just for clarity.
//This **REQUIRES** that the divider on the battery & BEMF circuits have the same ratio.
bemf[0] = adcGetVBatt() - adcGetBEMFL();
bemf[1] = adcGetVBatt() - adcGetBEMFR();
//NOTE: at this point, we should have a proper correspondance between
// the order of all the structured variable; bemf[i] associated with
// pidObjs[i], bemfLast[i], etc.
// Any "jumbling" of the inputs can be done in the above assignments.
//Negative ADC measures mean nothing and should never happen anyway
if (bemf[0] < 0) {
bemf[0] = 0;
}
if (bemf[1] < 0) {
bemf[1] = 0;
}
//Apply median filter
int i;
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
bemfHist[i][2] = bemfHist[i][1]; //rotate first
bemfHist[i][1] = bemfHist[i][0];
bemfHist[i][0] = bemf[i]; //include newest value
bemf[i] = medianFilter3(bemfHist[i]); //Apply median filter
}
// IIR filter on BEMF: y[n] = 0.2 * y[n-1] + 0.8 * x[n]
bemf[0] = (2 * (long) bemfLast[0] / 10) + 8 * (long) bemf[0] / 10;
bemf[1] = (2 * (long) bemfLast[1] / 10) + 8 * (long) bemf[1] / 10;
bemfLast[0] = bemf[0]; //bemfLast will not be used after here, OK to set
bemfLast[1] = bemf[1];
//Simple indicator if a leg is "in motion", via the yellow LED.
//Not functionally necceasry; can be elimited to use the LED for something else.
if ((bemf[0] > 0) || (bemf[1] > 0)) {
LED_YELLOW = 1;
} else {
LED_YELLOW = 0;
}
}
// Runs the PID controllers for the legs
void serviceMotionPID() {
//Apply steering mixing, without overwriting anything
int presteer[2] = {motor_pidObjs[0].input, motor_pidObjs[1].input};
int poststeer[2] = {0, 0};
steeringApplyCorrection(presteer, poststeer);
motor_pidObjs[0].input = poststeer[0];
motor_pidObjs[1].input = poststeer[1];
updateBEMF();
/////////// PID Section //////////
int j;
for (j = 0; j < NUM_MOTOR_PIDS; j++) {
//We are now measuring battery voltage directly via AN0,
// so the input offset to each PID loop can actually be tracked, and needs
// to be updated. This should compensate for battery voltage drooping over time.
motor_pidObjs[j].inputOffset = adcGetVBatt();
//pidobjs[0] : Left side
//pidobjs[0] : Right side
if (motor_pidObjs[j].onoff) {
//TODO: Do we want to add provisions to track error, even when
//the output is switched off?
#ifdef PID_SOFTWARE
//Update values
pidUpdate(&(motor_pidObjs[j]), bemf[j]);
#elif defined PID_HARDWARE
//Apply scaling, update, remove scaling for consistency
int temp;
temp = motor_pidObjs[j].input; //Save unscaled input val
motor_pidObjs[j].input *= MOTOR_PID_SCALER; //Scale input
pidUpdate(&(motor_pidObjs[j]), MOTOR_PID_SCALER* bemf[j]);
motor_pidObjs[j].input = temp; //Reset unscaled input
#endif //PID_SOFTWWARE vs PID_HARDWARE
//Set PWM duty cycle
SetDCMCPWM(legCtrlOutputChannels[j], motor_pidObjs[j].output, 0);
}//end of if (on / off)
else if (PID_ZEROING_ENABLE) { //if PID loop is off
SetDCMCPWM(legCtrlOutputChannels[j], 0, 0);
}
} // end of for(j)
}
