void cmdSetThrustClosedLoop(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame)
{ int thrust1 = frame[0] + (frame[1] << 8);
unsigned int run_time_ms1 = frame[2] + (frame[3] << 8);
int thrust2 = frame[4] + (frame[5] << 8);
unsigned int run_time_ms2 = frame[6] + (frame[7] << 8);
//currentMove = manualMove;
pidSetInput(0 ,thrust1, run_time_ms1);
pidOn(0);
pidSetInput(1 ,thrust2, run_time_ms2);
pidOn(1);
}
void serviceMoveQueue(void) {
//Blink red LED when executing move program
if (currentMove != idleMove) {
if (blinkCtr == 0) {
blinkCtr = 100;
LED_RED = ~LED_RED;
}
blinkCtr--;
}
//Service Move Queue if not empty
if (!mqIsEmpty(moveq)) {
inMotion = 1;
if ((currentMove == idleMove) || (getT1_ticks() >= moveExpire)) {
currentMove = mqPop(moveq);
//MOVE_SEG_LOOP_DECL only needs to appear once
if(currentMove->type == MOVE_SEG_LOOP_DECL){
mqLoopingOnOff(1);
currentMove = mqPop(moveq);
}
if(currentMove->type == MOVE_SEG_LOOP_CLEAR){
mqLoopingOnOff(0);
currentMove = mqPop(moveq);
}
if(currentMove->type == MOVE_SEG_QFLUSH){
while(mqPop(moveq)); //Terminate on NULL return, flushing queue
currentMove = idleMove;
}
//TODO: handle NULL return from mqPop
moveExpire = getT1_ticks() + currentMove->duration;
currentMoveStart = getT1_ticks();
//If we are no on an Idle move, turn on controllers
if (currentMove->type != MOVE_SEG_IDLE) {
motor_pidObjs[0].onoff = PID_ON;
motor_pidObjs[1].onoff = PID_ON;
}
}
} //Move Queue is empty
else if ((getT1_ticks() >= moveExpire) && currentMove != idleMove) {
//No more moves, go back to idle
currentMove = idleMove;
pidSetInput(&(motor_pidObjs[0]), 0);
motor_pidObjs[0].onoff = PID_OFF;
pidSetInput(&(motor_pidObjs[1]), 0);
motor_pidObjs[1].onoff = PID_OFF;
moveExpire = 0;
inMotion = 0; //for sleep, synthesis
steeringOff();
}
}
// -------------------------------------------
// called from main()
void pidSetup()
{
int i;
for(i = 0; i < NUM_PIDS; i++){
initPIDObjPos( &(pidObjs[i]), DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, DEFAULT_KAW, DEFAULT_FF);
}
initPIDVelProfile();
SetupTimer1(); // main interrupt used for leg motor PID
lastMoveTime = 0;
// initialize PID structures before starting Timer1
pidSetInput(0,0);
pidSetInput(1,0);
EnableIntT1; // turn on pid interrupts
calibBatteryOffset(100); //???This is broken for 2.5
}
void peHomingLoop(void) {
if(swatchToc() > 100000) {
swatchTic();
if (homingOn) {
peTrack();
if((++callCount) == CALL_FREQ) {
callCount = 0;
peSendFrame();
}
}
}
pidSetInput(0, motor_l, 1);
pidSetInput(1, motor_r, 1);
}
// ==== Flash/Experiment Commands ==============================================================================
// =============================================================================================================
unsigned char cmdStartTimedRun(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
unsigned int run_time = frame[0] + (frame[1] << 8);
int i;
for (i = 0; i < NUM_PIDS; i++){
pidObjs[i].timeFlag = 1;
pidSetInput(i, 0);
checkSwapBuff(i);
pidOn(i);
}
pidObjs[0].mode = 0;
pidStartTimedTrial(run_time);
return 1;
}
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;
}
}
void legCtrlSetInput(unsigned int num, int val){
pidSetInput(&(motor_pidObjs[num]), val);
}
