void telemGetPID(){
telemPIDdata.posL = pidObjs[0].p_state;
telemPIDdata.posR = pidObjs[1].p_state;
telemPIDdata.composL = pidObjs[0].p_input + pidObjs[0].interpolate;
telemPIDdata.composR = pidObjs[1].p_input + pidObjs[1].interpolate;
telemPIDdata.dcL = pidObjs[0].output; // left
telemPIDdata.dcR = pidObjs[1].output; // right
telemPIDdata.bemfL = bemf[0];
telemPIDdata.bemfR = bemf[1];
mpuGetGyro(gdata);
mpuGetXl(xldata);
telemPIDdata.gyroX = gdata[0];
telemPIDdata.gyroY = gdata[1];
telemPIDdata.gyroZ = gdata[2];
telemPIDdata.accelX = xldata[0];
telemPIDdata.accelY = xldata[1];
telemPIDdata.accelZ = xldata[2];
telemPIDdata.Vbatt = (int) adcGetVbatt();
// Save Data to flash
if (samplesToSave > 0) {
telemPIDdata.timestamp = sclockGetTime() - telemStartTime;
telemPIDdata.sampleIndex = sampIdx;
telemSaveData(&telemPIDdata);
sampIdx++;
}
return;
}
static void telemISRHandler() {
//skipcounter decrements to 0, triggering a telemetry save, and resets
// value of skicounter
if (samplesToSave > 0) {
telemBuffer.timestamp = sclockGetTime() - telemStartTime;
telemBuffer.sampleIndex = sampIdx;
//Write telemetry data into packet
//TELEMPACKFUNC((unsigned char*) &(telemBuffer.telemData));
TELEMPACKFUNC( &(telemBuffer.telemData) );
telemSaveData(&telemBuffer);
sampIdx++;
}
}
void pidGetState()
{ int i;
long p_state;
unsigned long time_start, time_end;
// calib_flag = 0; //BEMF disable
// get diff amp offset with motor off at startup time
if(calib_flag)
{
offsetAccumulatorL += adcGetMotorA();
offsetAccumulatorR += adcGetMotorB();
offsetAccumulatorCounter++; }
// choose velocity estimate
#if VEL_BEMF == 0 // use first difference on position for velocity estimate
long oldpos[NUM_PIDS], velocity;
for(i=0; i<NUM_PIDS; i++)
{ oldpos[i] = pidObjs[i].p_state; }
#endif
time_start = sclockGetTime();
bemf[0] = pidObjs[0].inputOffset - adcGetMotorA(); // watch sign for A/D? unsigned int -> signed?
bemf[1] = pidObjs[1].inputOffset - adcGetMotorB(); // MotorB
// only works to +-32K revs- might reset after certain number of steps? Should wrap around properly
for(i =0; i<NUM_PIDS; i++)
{ p_state = (long)(encPos[i].pos << 2); // pos 14 bits 0x0 -> 0x3fff
p_state = p_state + (encPos[i].oticks << 16);
p_state = p_state - (long)(encPos[i].offset <<2); // subtract offset to get zero position
if (i==0)
{
pidObjs[i].p_state = p_state; //fix for encoder alignment
}
else
{
pidObjs[i].p_state = -p_state;
}
}
time_end = sclockGetTime() - time_start;
#if VEL_BEMF == 0 // use first difference on position for velocity estimate
for(i=0; i<NUM_PIDS; i++)
{ velocity = pidObjs[i].p_state - oldpos[i]; // Encoder ticks per ms
if (velocity > 0x7fff) velocity = 0x7fff; // saturate to int
if(velocity < -0x7fff) velocity = -0x7fff;
pidObjs[i].v_state = (int) velocity;
}
#endif
// choose velocity estimate
#if VEL_BEMF == 1
int measurements[NUM_PIDS];
// Battery: AN0, MotorA AN8, MotorB AN9, MotorC AN10, MotorD AN11
measurements[0] = pidObjs[0].inputOffset - adcGetMotorA(); // watch sign for A/D? unsigned int -> signed?
measurements[1] = pidObjs[1].inputOffset - adcGetMotorB(); // MotorB
//Get motor speed reading on every interrupt - A/D conversion triggered by PWM timer to read Vm when transistor is off
// when motor is loaded, sometimes see motor short so that bemf=offset voltage
// get zero sometimes - open circuit brush? Hence try median filter
for(i = 0; i<2; i++) // median filter
{ if(measurements[i] > measLast1[i])
{ if(measLast1[i] > measLast2[i]) {bemf[i] = measLast1[i];} // middle value is median
else // middle is smallest
{ if(measurements[i] > measLast2[i]) {bemf[i] = measLast2[i];} // 3rd value is median
else{ bemf[i] = measurements[i];} // first value is median
}
}
else // first is not biggest
{ if(measLast1[i] < measLast2[i]) {bemf[i] = measLast1[i];} // middle value is median
else // middle is biggest
{ if(measurements[i] < measLast2[i]) {bemf[i] = measLast2[i];} // 3rd value is median
else
{ bemf[i] = measurements[i]; // first value is median
}
}
}
} // end for
// store old values
measLast2[0] = measLast1[0]; measLast1[0] = measurements[0];
measLast2[1] = measLast1[1]; measLast1[1] = measurements[1];
pidObjs[0].v_state = bemf[0];
pidObjs[1].v_state = bemf[1]; // might also estimate from deriv of pos data
//if((measurements[0] > 0) || (measurements[1] > 0)) {
if((measurements[0] > 0)) { LED_BLUE = 1;}
else{ LED_BLUE = 0;}
#endif
}
void telemSetStartTime(void) {
telemStartTime = sclockGetTime();
}
