static void hallSetControl() {
int j;
// 0 = right side
for (j = 0; j < NUM_HALL_PIDS; j++) { //pidobjs[0] : right side
// p_input has scaled velocity interpolation to make smoother
hallPIDObjs[j].p_error = hallPIDObjs[j].p_input + (hallPIDVel[j].interpolate >> 8) - motor_count[j];
//hallPIDObjs[j].v_error = hallPIDObjs[j].v_input - measurements[j];
hallPIDObjs[j].v_error = hallPIDObjs[j].v_input - hallbemf[j];
//Update values
hallUpdatePID(&(hallPIDObjs[j]));
if (hallPIDObjs[j].onoff) {
//Might want to change this in the future, if we want to track error
//even when the motor is off.
//Set PWM duty cycle
if (j == 0) { // PWM1.L
SetDCMCPWM(MC_CHANNEL_PWM1, hallPIDObjs[0].output, 0); //PWM1.L
} else if (j == 1) { // PWM2.l
SetDCMCPWM(MC_CHANNEL_PWM2, hallPIDObjs[1].output, 0); // PWM2.L
}
}//end of if (on / off)
else { //if PID loop is off
if (j == 0) {
SetDCMCPWM(MC_CHANNEL_PWM1, 0, 0);
} else if (j == 1) {
SetDCMCPWM(MC_CHANNEL_PWM2, 0, 0);
}
}
} // end of for(j)
}
void SetupPWM(void)
{
#if (defined(__IMAGEPROC1) || defined(__IMAGEPROC2))
#if defined(__IMAGEPROC1)
PTPER = 2499; // Fcy/(Fpwm * Prescale) - 1
SEVTCMP = 2490; //620; // Special Event Trigger Compare Value for ADC in phase with PWM
PWMCON1 = PWM_MOD2_IND & PWM_PEN2L & PWM_MOD3_IND & PWM_PEN3L;
SetDCMCPWM(2, 0, 0);
SetDCMCPWM(3, 0, 0);
#elif defined(__IMAGEPROC2)
PTPER = 624; // Fcy/(Fpwm * Prescale) - 1
SEVTCMP = 620; // Special Event Trigger Compare Value for ADC in phase with PWM
PWMCON1 = PWM_MOD1_IND & PWM_PEN1L;
SetDCMCPWM(1, 0, 0);
#endif
PWMCON2 = PWM_SEVOPS1 & PWM_OSYNC_TCY & PWM_UEN;
PTCON = PWM_EN & PWM_IDLE_STOP & PWM_OP_SCALE1 & PWM_IPCLK_SCALE64 &
PWM_MOD_FREE;
DisableIntMCPWM; DisableIntFLTA; DisableIntFLTB;
#endif // (defined(__IMAGEPROC1) || defined(__IMAGEPROC2))
}
void EmergencyStop(void)
{
pidSetInput(0 ,0, 0);
pidSetInput(1,0,0);
DisableIntT1; // turn off pid interrupts
SetDCMCPWM(MC_CHANNEL_PWM1, 0, 0); // set PWM to zero
SetDCMCPWM(MC_CHANNEL_PWM2, 0, 0);
}
void mcSetDutyCycle(unsigned char channel, float duty_cycle) {
unsigned int pdc_value;
pdc_value = (unsigned int)(2*duty_cycle/100*(10000));
SetDCMCPWM(channel, pdc_value, 0);
}
static void batDefaultCallback(void) {
unsigned char i;
// Disable all interrupts
CRITICAL_SECTION_START;
LED_1 = 1;
LED_2 = 1;
#if defined(__IMAGEPROC2)
LED_3 = 1;
#endif
#if defined(__LOWBATT_STOPS_MOTORS)
// Stop any running motors
for (i=1; i<=4; i++) { SetDCMCPWM(i, 0, 0); }
CloseMCPWM();
#endif
// Slowly blink all LEDs 5 times (1 second interval)
for (i=0; i<5; ++i) {
LED_1 = ~LED_1;
LED_2 = ~LED_2;
#if defined(__IMAGEPROC2)
LED_3 = ~LED_3;
#endif
delay_ms(1000);
}
CRITICAL_SECTION_END;
}
// 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)
}
void SetupPWM(void){
LATB = 0x0000;
TRISB = 0b0000011011111011;
PORTBbits.RB8 = 0;
PORTBbits.RB11 = 0;
unsigned int SEVTCMPvalue, PTCONvalue, PWMCON1value, PWMCON2value;
SEVTCMPvalue = 160; //Special event compare register triggers special event
//SEVTCMPvalue = 4839; //Special event compare register triggers special event
//at this point in the PWM period. Useful for sampling
//back EMF when motor is turned off, for example.
//Values larger than 4839 for a time base of 4999 give
//unreliable results. Consequently, running the motor
//at greater than 96.7% duty cycle means you cannot
//sense back EMF.
PTCONvalue = PWM_EN & PWM_IDLE_CON & PWM_OP_SCALE1 &
// PWM_IPCLK_SCALE4 & PWM_MOD_FREE;
PWM_IPCLK_SCALE16 & PWM_MOD_FREE;
PWMCON1value = PWM_MOD1_IND & PWM_PEN1L & PWM_PEN1H &
PWM_MOD2_IND & PWM_PEN2L & PWM_PEN2H &
PWM_MOD3_IND & PWM_PEN3L & PWM_PEN3H;
PWMCON2value = PWM_SEVOPS1 & PWM_OSYNC_TCY & PWM_UEN;
ConfigIntMCPWM(PWM_INT_DIS & PWM_FLTA_DIS_INT & PWM_FLTB_DIS_INT);
OpenMCPWM(PTPERvalue, SEVTCMPvalue, PTCONvalue, PWMCON1value, PWMCON2value);
P1OVDCONbits.POVD1L = 1;
P1OVDCONbits.POVD1H = 1;
P1OVDCONbits.POVD2L = 1;
P1OVDCONbits.POVD2H = 1;
P1OVDCONbits.POVD3L = 1;
P1OVDCONbits.POVD3H = 1;
SetDCMCPWM(1, 0, 0);
SetDCMCPWM(2, 0, 0);
SetDCMCPWM(3, 0, 0);
}
void tiHSetDC(unsigned int channel, int dutycycle) {
unsigned int idx = channel - 1;
if (dutycycle > max_pwm) dutycycle = max_pwm;
if (dutycycle < -max_pwm) dutycycle = -max_pwm;
outputs[idx].throt_f = -666.0; //TODO: not a solution; have to update float every time?
outputs[idx].throt_i = dutycycle;
if (dutycycle < 0) {
outputs[idx].dir = TIH_REV;
dutycycle = -dutycycle;
} else {
outputs[idx].dir = TIH_FWD;
}
//Select correct PWM output and GPIO level for dir and mode
tiHConfigure(channel);
//Set duty cycle max = 0xfff
SetDCMCPWM(channel, dutycycle, 0);
}
void tiHSetFloat(unsigned int channel, float percent) {
unsigned int idx = channel - 1;
int pdc_value;
pdc_value = (int) (2 * percent / 100 * (PTPER));
outputs[idx].throt_f = percent;
outputs[idx].throt_i = ABS(pdc_value);
if (pdc_value < 0) {
outputs[idx].dir = TIH_REV;
pdc_value = ABS(pdc_value);
}
else {
outputs[idx].dir = TIH_FWD;
}
//Select correct PWM output and GPIO level for dir and mode
tiHConfigure(channel);
//Set duty cycle
SetDCMCPWM(channel, pdc_value, 0);
}
static void mcSetupPeripheral(void) {
//unsigned int PTPERvalue = 10000;
unsigned int PTPERvalue = 2000;
//unsigned int PTPERvalue = 2000;
unsigned int SEVTCMPvalue, PTCONvalue, PWMCON1value, PWMCON2value;
SEVTCMPvalue = 1920;
SEVTCMPvalue = 9940;
SEVTCMPvalue = 1988; // ok up to 96% of time base, according to Aaron
// SEVTCMPvalue = 160; // Special Event Trigger Compare Value for ADC in phase with PWM
//PTCONvalue = PWM_EN & PWM_IDLE_CON & PWM_OP_SCALE1 &
// PWM_IPCLK_SCALE4 & PWM_MOD_FREE;
PTCONvalue = PWM_EN & PWM_IDLE_CON & PWM_OP_SCALE1 &
PWM_IPCLK_SCALE1 & PWM_MOD_FREE;
PWMCON1value = PWM_MOD1_IND & PWM_PEN1L & PWM_MOD2_IND & PWM_PEN2L &
PWM_MOD3_IND & PWM_PEN3L & PWM_MOD4_IND & PWM_PEN4L;
PWMCON2value = PWM_SEVOPS1 & PWM_OSYNC_TCY & PWM_UEN;
ConfigIntMCPWM(PWM_INT_DIS & PWM_FLTA_DIS_INT & PWM_FLTB_DIS_INT);
SetDCMCPWM(1, 0, 0);
OpenMCPWM(PTPERvalue, SEVTCMPvalue, PTCONvalue, PWMCON1value, PWMCON2value);
SetDCMCPWM(2, 0, 0);
OpenMCPWM(PTPERvalue, SEVTCMPvalue, PTCONvalue, PWMCON1value, PWMCON2value);
//Stan's
// For 1KHz at MIPS == 40
/*
pwm_period_ = 624;
ConfigIntMCPWM(PWM_INT_DIS & PWM_FLTA_DIS_INT & PWM_FLTB_DIS_INT);
PTPER = pwm_period_;
PWMCON1bits.PMOD1 = 1;
PWMCON1bits.PMOD2 = 1;
PWMCON1bits.PMOD3 = 0;
PWMCON1bits.PMOD4 = 0;
PWMCON1bits.PEN1H = 0;
PWMCON1bits.PEN2H = 0;
PWMCON1bits.PEN3H = 0;
PWMCON1bits.PEN4H = 0;
PWMCON1bits.PEN1L = 1;
PWMCON1bits.PEN2L = 1;
PWMCON1bits.PEN3L = 0;
PWMCON1bits.PEN4L = 0;
PWMCON2bits.SEVOPS = 1; // postscale 1:1
PWMCON2bits.OSYNC = 0;
PWMCON2bits.IUE = 1;
PWMCON2 |= PWM_UEN;
P1SECMPbits.SEVTDIR = 1;
PTCONbits.PTMOD = 0; // Free running mode
PTCONbits.PTOPS = 0b11; // postscale 1:1
PTCONbits.PTCKPS = 0b11; // postscale 1:64
PTCONbits.PTSIDL = 0; // runs in CPU idle mode
PTCONbits.PTEN = 1;
PDC1 = 0x35; // duty cycle = 0
PDC2 = 0x35;
SEVTCMP = 600; // Special Event Trigger Compare Value for ADC in phase with PWM
//P1SECMPbits.SEVTCMP = 600;
AD1CON1bits.SSRC = 0b011;
*/
}
