void steeringSetup(void) {
#ifdef PID_HARDWARE
//Create PID controller object
steeringPID.dspPID.abcCoefficients = steering_abcCoeffs;
steeringPID.dspPID.controlHistory = steering_controlHists;
#endif
pidInitPIDObj(&steeringPID, STEERING_KP, STEERING_KI, STEERING_KD, STEERING_KAW, 0);
steeringPID.satValPos = STEERING_SAT;
steeringPID.satValNeg = -STEERING_SAT;
steeringPID.maxVal = STEERING_SAT;
steeringPID.minVal = -STEERING_SAT;
steeringSetInput(0);
SetupTimer6(); //T5 ISR will update the steering controller
int retval;
retval = sysServiceInstallT6(steeringServiceRoutine);
//Averaging filter setup:
//filterAvgCreate(&gyroZavg, GYRO_AVG_SAMPLES);
//This is now owned by imu.c
steeringPID.onoff = PID_OFF; //OFF by default
steeringMode = STEERMODE_DECREASE;
}
void legCtrlSetup() {
int i;
//Setup for PID controllers
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
#ifdef PID_HARDWARE
//THe user is REQUIRED to set up these pointers before initializing
//the object, because the arrays are local to this module.
motor_pidObjs[i].dspPID.abcCoefficients =
motor_abcCoeffs[i];
motor_pidObjs[i].dspPID.controlHistory =
motor_controlHists[i];
#endif
pidInitPIDObj(&(motor_pidObjs[i]), LEG_DEFAULT_KP, LEG_DEFAULT_KI,
LEG_DEFAULT_KD, LEG_DEFAULT_KAW, LEG_DEFAULT_KFF);
//Set up max's and saturation values
motor_pidObjs[i].satValPos = SATTHROT;
motor_pidObjs[i].satValNeg = 0;
motor_pidObjs[i].maxVal = FULLTHROT;
motor_pidObjs[i].minVal = 0;
}
//Set which PWM output each PID Object will correspond to
legCtrlOutputChannels[0] = MC_CHANNEL_PWM1;
legCtrlOutputChannels[1] = MC_CHANNEL_PWM2;
SetupTimer1(); // Timer 1 @ 1 Khz
int retval;
retval = sysServiceInstallT1(legCtrlServiceRoutine);
//ADC_OffsetL = 1; //prevent divide by zero errors
//ADC_OffsetR = 1;
//Move Queue setup and initialization
moveq = mqInit(32);
idleMove = malloc(sizeof (moveCmdStruct));
idleMove->inputL = 0;
idleMove->inputR = 0;
idleMove->duration = 0;
idleMove->type = MOVE_SEG_IDLE;
idleMove->params[0] = 0;
idleMove->params[1] = 0;
idleMove->params[2] = 0;
currentMove = idleMove;
currentMoveStart = 0;
moveExpire = 0;
blinkCtr = 0;
inMotion = 0;
//Ensure controllers are reset to zero and turned off
//External function used here since it will zero out the state
pidSetInput(&(motor_pidObjs[0]), 0);
pidSetInput(&(motor_pidObjs[1]), 0);
motor_pidObjs[0].onoff = PID_OFF;
motor_pidObjs[1].onoff = PID_OFF;
//Set up filters and histories
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
bemfLast[i] = 0;
bemfHist[i][0] = 0;
bemfHist[i][1] = 0;
bemfHist[i][2] = 0;
}
}
